Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Remove athena hal folder (#7668)

Browse Source 
Also remove roborio bazel target.
This commit is contained in:
Thad House
2025-01-13 11:25:28 -08:00
committed by GitHub
parent 09a6bc9a25
commit df77580a15
84 changed files with 0 additions and 26708 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.github/workflows/bazel.yml vendored
View File

@@ -57,7 +57,6 @@ jobs:
matrix:
include:
- { name: "Linux (native)", os: ubuntu-24.04, action: "test", config: "--config=linux", }
- { name: "Linux (roborio)", os: ubuntu-24.04, action: "build", config: "--config=roborio", }
name: "${{ matrix.name }}"
runs-on: ${{ matrix.os }}
steps:

246
hal/src/main/native/athena/Accelerometer.cpp
View File

@@ -1,246 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Accelerometer.h"
#include <stdint.h>
#include <cassert>
#include <cstdio>
#include <memory>
#include "HALInitializer.h"
#include "hal/ChipObject.h"
#include "hal/HAL.h"
using namespace hal;
// The 7-bit I2C address with a 0 "send" bit
static constexpr uint8_t kSendAddress = (0x1c << 1) | 0;
// The 7-bit I2C address with a 1 "receive" bit
static constexpr uint8_t kReceiveAddress = (0x1c << 1) | 1;
static constexpr uint8_t kControlTxRx = 1;
static constexpr uint8_t kControlStart = 2;
static constexpr uint8_t kControlStop = 4;
static std::unique_ptr<tAccel> accel;
static HAL_AccelerometerRange accelerometerRange;
// Register addresses
enum Register {
kReg_Status = 0x00,
kReg_OutXMSB = 0x01,
kReg_OutXLSB = 0x02,
kReg_OutYMSB = 0x03,
kReg_OutYLSB = 0x04,
kReg_OutZMSB = 0x05,
kReg_OutZLSB = 0x06,
kReg_Sysmod = 0x0B,
kReg_IntSource = 0x0C,
kReg_WhoAmI = 0x0D,
kReg_XYZDataCfg = 0x0E,
kReg_HPFilterCutoff = 0x0F,
kReg_PLStatus = 0x10,
kReg_PLCfg = 0x11,
kReg_PLCount = 0x12,
kReg_PLBfZcomp = 0x13,
kReg_PLThsReg = 0x14,
kReg_FFMtCfg = 0x15,
kReg_FFMtSrc = 0x16,
kReg_FFMtThs = 0x17,
kReg_FFMtCount = 0x18,
kReg_TransientCfg = 0x1D,
kReg_TransientSrc = 0x1E,
kReg_TransientThs = 0x1F,
kReg_TransientCount = 0x20,
kReg_PulseCfg = 0x21,
kReg_PulseSrc = 0x22,
kReg_PulseThsx = 0x23,
kReg_PulseThsy = 0x24,
kReg_PulseThsz = 0x25,
kReg_PulseTmlt = 0x26,
kReg_PulseLtcy = 0x27,
kReg_PulseWind = 0x28,
kReg_ASlpCount = 0x29,
kReg_CtrlReg1 = 0x2A,
kReg_CtrlReg2 = 0x2B,
kReg_CtrlReg3 = 0x2C,
kReg_CtrlReg4 = 0x2D,
kReg_CtrlReg5 = 0x2E,
kReg_OffX = 0x2F,
kReg_OffY = 0x30,
kReg_OffZ = 0x31
};
namespace hal::init {
void InitializeAccelerometer() {}
} // namespace hal::init
namespace hal {
static void writeRegister(Register reg, uint8_t data);
static uint8_t readRegister(Register reg);
/**
* Initialize the accelerometer.
*/
static void initializeAccelerometer() {
hal::init::CheckInit();
int32_t status = 0;
if (!accel) {
accel.reset(tAccel::create(&status));
accelerometerRange = HAL_AccelerometerRange::HAL_AccelerometerRange_k2G;
// Enable I2C
accel->writeCNFG(1, &status);
// Set the counter to 100 kbps
accel->writeCNTR(213, &status);
// The device identification number should be 0x2a
assert(readRegister(kReg_WhoAmI) == 0x2a);
}
}
static void writeRegister(Register reg, uint8_t data) {
int32_t status = 0;
uint64_t initialTime;
accel->writeADDR(kSendAddress, &status);
// Send a start transmit/receive message with the register address
accel->writeCNTL(kControlStart | kControlTxRx, &status);
accel->writeDATO(reg, &status);
accel->strobeGO(&status);
// Execute and wait until it's done (up to a millisecond)
initialTime = HAL_GetFPGATime(&status);
while (accel->readSTAT(&status) & 1) {
if (HAL_GetFPGATime(&status) > initialTime + 1000) {
break;
}
}
// Send a stop transmit/receive message with the data
accel->writeCNTL(kControlStop | kControlTxRx, &status);
accel->writeDATO(data, &status);
accel->strobeGO(&status);
// Execute and wait until it's done (up to a millisecond)
initialTime = HAL_GetFPGATime(&status);
while (accel->readSTAT(&status) & 1) {
if (HAL_GetFPGATime(&status) > initialTime + 1000) {
break;
}
}
}
static uint8_t readRegister(Register reg) {
int32_t status = 0;
uint64_t initialTime;
// Send a start transmit/receive message with the register address
accel->writeADDR(kSendAddress, &status);
accel->writeCNTL(kControlStart | kControlTxRx, &status);
accel->writeDATO(reg, &status);
accel->strobeGO(&status);
// Execute and wait until it's done (up to a millisecond)
initialTime = HAL_GetFPGATime(&status);
while (accel->readSTAT(&status) & 1) {
if (HAL_GetFPGATime(&status) > initialTime + 1000) {
break;
}
}
// Receive a message with the data and stop
accel->writeADDR(kReceiveAddress, &status);
accel->writeCNTL(kControlStart | kControlStop | kControlTxRx, &status);
accel->strobeGO(&status);
// Execute and wait until it's done (up to a millisecond)
initialTime = HAL_GetFPGATime(&status);
while (accel->readSTAT(&status) & 1) {
if (HAL_GetFPGATime(&status) > initialTime + 1000) {
break;
}
}
return accel->readDATI(&status);
}
/**
* Convert a 12-bit raw acceleration value into a scaled double in units of
* 1 g-force, taking into account the accelerometer range.
*/
static double unpackAxis(int16_t raw) {
// The raw value is actually 12 bits, not 16, so we need to propagate the
// 2's complement sign bit to the unused 4 bits for this to work with
// negative numbers.
raw <<= 4;
raw >>= 4;
switch (accelerometerRange) {
case HAL_AccelerometerRange_k2G:
return raw / 1024.0;
case HAL_AccelerometerRange_k4G:
return raw / 512.0;
case HAL_AccelerometerRange_k8G:
return raw / 256.0;
default:
return 0.0;
}
}
} // namespace hal
extern "C" {
void HAL_SetAccelerometerActive(HAL_Bool active) {
initializeAccelerometer();
uint8_t ctrlReg1 = readRegister(kReg_CtrlReg1);
ctrlReg1 &= ~1; // Clear the existing active bit
writeRegister(kReg_CtrlReg1, ctrlReg1 | (active ? 1 : 0));
}
void HAL_SetAccelerometerRange(HAL_AccelerometerRange range) {
initializeAccelerometer();
accelerometerRange = range;
uint8_t xyzDataCfg = readRegister(kReg_XYZDataCfg);
xyzDataCfg &= ~3; // Clear the existing two range bits
writeRegister(kReg_XYZDataCfg, xyzDataCfg | range);
}
double HAL_GetAccelerometerX(void) {
initializeAccelerometer();
int32_t raw =
(readRegister(kReg_OutXMSB) << 4) | (readRegister(kReg_OutXLSB) >> 4);
return unpackAxis(raw);
}
double HAL_GetAccelerometerY(void) {
initializeAccelerometer();
int32_t raw =
(readRegister(kReg_OutYMSB) << 4) | (readRegister(kReg_OutYLSB) >> 4);
return unpackAxis(raw);
}
double HAL_GetAccelerometerZ(void) {
initializeAccelerometer();
int32_t raw =
(readRegister(kReg_OutZMSB) << 4) | (readRegister(kReg_OutZLSB) >> 4);
return unpackAxis(raw);
}
} // extern "C"

264
hal/src/main/native/athena/AddressableLED.cpp
View File

@@ -1,264 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/AddressableLED.h"
#include <cstring>
#include <memory>
#include <fmt/format.h>
#include "ConstantsInternal.h"
#include "DigitalInternal.h"
#include "FPGACalls.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/AddressableLEDTypes.h"
#include "hal/ChipObject.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
using namespace hal;
namespace {
struct AddressableLED {
std::unique_ptr<tLED> led;
void* ledBuffer;
size_t ledBufferSize;
int32_t stringLength = 1;
};
} // namespace
static LimitedHandleResource<
HAL_AddressableLEDHandle, AddressableLED, kNumAddressableLEDs,
HAL_HandleEnum::AddressableLED>* addressableLEDHandles;
namespace hal::init {
void InitializeAddressableLED() {
static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
kNumAddressableLEDs,
HAL_HandleEnum::AddressableLED>
alH;
addressableLEDHandles = &alH;
}
} // namespace hal::init
static constexpr const char* HmbName = "HMB_0_LED";
extern "C" {
HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
HAL_DigitalHandle outputPort, int32_t* status) {
hal::init::CheckInit();
auto digitalPort =
hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
if (!digitalPort) {
// If DIO was passed, channel error, else generic error
if (getHandleType(outputPort) == hal::HAL_HandleEnum::DIO) {
*status = HAL_LED_CHANNEL_ERROR;
} else {
*status = HAL_HANDLE_ERROR;
}
return HAL_kInvalidHandle;
}
if (digitalPort->channel >= kNumPWMHeaders) {
*status = HAL_LED_CHANNEL_ERROR;
return HAL_kInvalidHandle;
}
auto handle = addressableLEDHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
led->led.reset(tLED::create(status));
if (*status != 0) {
addressableLEDHandles->Free(handle);
return HAL_kInvalidHandle;
}
led->led->writeOutputSelect(digitalPort->channel, status);
if (*status != 0) {
addressableLEDHandles->Free(handle);
return HAL_kInvalidHandle;
}
led->ledBuffer = nullptr;
led->ledBufferSize = 0;
uint32_t session = led->led->getSystemInterface()->getHandle();
*status = hal::HAL_NiFpga_OpenHmb(session, HmbName, &led->ledBufferSize,
&led->ledBuffer);
if (*status != 0) {
addressableLEDHandles->Free(handle);
return HAL_kInvalidHandle;
}
return handle;
}
void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
return;
}
uint32_t session = led->led->getSystemInterface()->getHandle();
hal::HAL_NiFpga_CloseHmb(session, HmbName);
addressableLEDHandles->Free(handle);
}
void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
HAL_DigitalHandle outputPort,
int32_t* status) {
auto digitalPort =
hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
if (!digitalPort) {
*status = HAL_HANDLE_ERROR;
return;
}
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
led->led->writeOutputSelect(digitalPort->channel, status);
}
void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
int32_t length, int32_t* status) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
if (length > HAL_kAddressableLEDMaxLength || length < 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format(
"LED length must be less than or equal to {}. {} was requested",
HAL_kAddressableLEDMaxLength, length));
return;
}
led->led->strobeReset(status);
while (led->led->readPixelWriteIndex(status) != 0) {
}
if (*status != 0) {
return;
}
led->led->writeStringLength(length, status);
led->stringLength = length;
}
static_assert(sizeof(HAL_AddressableLEDData) == sizeof(uint32_t),
"LED Data must be 32 bit");
void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
const struct HAL_AddressableLEDData* data,
int32_t length, int32_t* status) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
if (length > led->stringLength || length < 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format(
"Data length must be less than or equal to {}. {} was requested",
led->stringLength, length));
return;
}
if (length == 0) {
return;
}
std::memcpy(led->ledBuffer, data, length * sizeof(HAL_AddressableLEDData));
asm("dmb");
led->led->strobeLoad(status);
}
void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
int32_t highTime0NanoSeconds,
int32_t lowTime0NanoSeconds,
int32_t highTime1NanoSeconds,
int32_t lowTime1NanoSeconds,
int32_t* status) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
led->led->writeLowBitTickTiming(0, highTime0NanoSeconds / 25, status);
led->led->writeLowBitTickTiming(1, lowTime0NanoSeconds / 25, status);
led->led->writeHighBitTickTiming(0, highTime1NanoSeconds / 25, status);
led->led->writeHighBitTickTiming(1, lowTime1NanoSeconds / 25, status);
}
void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
int32_t syncTimeMicroSeconds,
int32_t* status) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
led->led->writeSyncTiming(syncTimeMicroSeconds, status);
}
void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
int32_t* status) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
led->led->strobeStart(status);
}
void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
int32_t* status) {
auto led = addressableLEDHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
led->led->strobeAbort(status);
}
} // extern "C"

136
hal/src/main/native/athena/AnalogAccumulator.cpp
View File

@@ -1,136 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/AnalogAccumulator.h"
#include "AnalogInternal.h"
#include "hal/HAL.h"
using namespace hal;
namespace hal::init {
void InitializeAnalogAccumulator() {}
} // namespace hal::init
extern "C" {
HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
for (int32_t i = 0; i < kNumAccumulators; i++) {
if (port->channel == kAccumulatorChannels[i]) {
return true;
}
}
return false;
}
void HAL_InitAccumulator(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
if (!HAL_IsAccumulatorChannel(analogPortHandle, status)) {
*status = HAL_INVALID_ACCUMULATOR_CHANNEL;
return;
}
HAL_SetAccumulatorCenter(analogPortHandle, 0, status);
HAL_ResetAccumulator(analogPortHandle, status);
}
void HAL_ResetAccumulator(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (port->accumulator == nullptr) {
*status = NULL_PARAMETER;
return;
}
port->accumulator->strobeReset(status);
}
void HAL_SetAccumulatorCenter(HAL_AnalogInputHandle analogPortHandle,
int32_t center, int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (port->accumulator == nullptr) {
*status = NULL_PARAMETER;
return;
}
port->accumulator->writeCenter(center, status);
}
void HAL_SetAccumulatorDeadband(HAL_AnalogInputHandle analogPortHandle,
int32_t deadband, int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (port->accumulator == nullptr) {
*status = NULL_PARAMETER;
return;
}
port->accumulator->writeDeadband(deadband, status);
}
int64_t HAL_GetAccumulatorValue(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (port->accumulator == nullptr) {
*status = NULL_PARAMETER;
return 0;
}
int64_t value = port->accumulator->readOutput_Value(status);
return value;
}
int64_t HAL_GetAccumulatorCount(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (port->accumulator == nullptr) {
*status = NULL_PARAMETER;
return 0;
}
return port->accumulator->readOutput_Count(status);
}
void HAL_GetAccumulatorOutput(HAL_AnalogInputHandle analogPortHandle,
int64_t* value, int64_t* count, int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (port->accumulator == nullptr) {
*status = NULL_PARAMETER;
return;
}
if (value == nullptr || count == nullptr) {
*status = NULL_PARAMETER;
return;
}
tAccumulator::tOutput output = port->accumulator->readOutput(status);
*value = output.Value;
*count = output.Count;
}
} // extern "C"

290
hal/src/main/native/athena/AnalogGyro.cpp
View File

@@ -1,290 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/AnalogGyro.h"
#include <cmath>
#include <string>
#include <thread>
#include <wpi/print.h>
#include "AnalogInternal.h"
#include "HALInitializer.h"
#include "HALInternal.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;
std::string previousAllocation;
};
} // namespace
static constexpr uint32_t kOversampleBits = 10;
static constexpr uint32_t kAverageBits = 0;
static constexpr double kSamplesPerSecond = 50.0;
static constexpr double kCalibrationSampleTime = 5.0;
static constexpr double kDefaultVoltsPerDegreePerSecond = 0.007;
using namespace hal;
static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
HAL_HandleEnum::AnalogGyro>* analogGyroHandles;
namespace hal::init {
void InitializeAnalogGyro() {
static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
HAL_HandleEnum::AnalogGyro>
agHandles;
analogGyroHandles = &agHandles;
}
} // namespace hal::init
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,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
// Handle will be type checked by HAL_IsAccumulatorChannel
int16_t channel = getHandleIndex(analogHandle);
if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
if (*status == 0) {
*status = HAL_INVALID_ACCUMULATOR_CHANNEL;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
0, kNumAccumulators, channel);
}
return HAL_kInvalidHandle;
}
HAL_GyroHandle handle;
auto gyro = analogGyroHandles->Allocate(channel, &handle, status);
if (*status != 0) {
if (gyro) {
hal::SetLastErrorPreviouslyAllocated(status, "Analog Gyro", channel,
gyro->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
0, kNumAccumulators, channel);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
// Initialize port structure
gyro->handle = analogHandle;
gyro->voltsPerDegreePerSecond = 0;
gyro->offset = 0;
gyro->center = 0;
gyro->previousAllocation = allocationLocation ? allocationLocation : "";
return handle;
}
void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
gyro->voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
HAL_SetAnalogAverageBits(gyro->handle, kAverageBits, status);
if (*status != 0) {
return;
}
HAL_SetAnalogOversampleBits(gyro->handle, kOversampleBits, status);
if (*status != 0) {
return;
}
double sampleRate =
kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
HAL_SetAnalogSampleRate(sampleRate, status);
if (*status != 0) {
return;
}
Wait(0.1);
HAL_SetAnalogGyroDeadband(handle, 0.0, status);
if (*status != 0) {
return;
}
}
void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {
analogGyroHandles->Free(handle);
}
void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle,
double voltsPerDegreePerSecond, double offset,
int32_t center, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
gyro->offset = offset;
gyro->center = center;
HAL_SetAccumulatorCenter(gyro->handle, center, status);
}
void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle,
double voltsPerDegreePerSecond,
int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
}
void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
HAL_ResetAccumulator(gyro->handle, status);
if (*status != 0) {
return;
}
const double sampleTime = 1.0 / HAL_GetAnalogSampleRate(status);
const double overSamples =
1 << HAL_GetAnalogOversampleBits(gyro->handle, status);
const double averageSamples =
1 << HAL_GetAnalogAverageBits(gyro->handle, status);
if (*status != 0) {
return;
}
Wait(sampleTime * overSamples * averageSamples);
}
void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
HAL_InitAccumulator(gyro->handle, status);
if (*status != 0) {
return;
}
wpi::print("Calibrating analog gyro for {} seconds.\n",
kCalibrationSampleTime);
Wait(kCalibrationSampleTime);
int64_t value;
int64_t count;
HAL_GetAccumulatorOutput(gyro->handle, &value, &count, status);
if (*status != 0) {
return;
}
gyro->center =
std::round(static_cast<double>(value) / static_cast<double>(count));
gyro->offset = static_cast<double>(value) / static_cast<double>(count) -
static_cast<double>(gyro->center);
HAL_SetAccumulatorCenter(gyro->handle, gyro->center, status);
if (*status != 0) {
return;
}
HAL_ResetAnalogGyro(handle, status);
}
void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts,
int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t deadband = static_cast<int32_t>(
volts * 1e9 / HAL_GetAnalogLSBWeight(gyro->handle, status) *
(1 << HAL_GetAnalogOversampleBits(gyro->handle, status)));
if (*status != 0) {
return;
}
HAL_SetAccumulatorDeadband(gyro->handle, deadband, status);
}
double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
int64_t rawValue = 0;
int64_t count = 0;
HAL_GetAccumulatorOutput(gyro->handle, &rawValue, &count, status);
int64_t value = rawValue - static_cast<int64_t>(static_cast<double>(count) *
gyro->offset);
double scaledValue =
value * 1e-9 *
static_cast<double>(HAL_GetAnalogLSBWeight(gyro->handle, status)) *
static_cast<double>(1 << HAL_GetAnalogAverageBits(gyro->handle, status)) /
(HAL_GetAnalogSampleRate(status) * gyro->voltsPerDegreePerSecond);
return scaledValue;
}
double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return (HAL_GetAnalogAverageValue(gyro->handle, status) -
(static_cast<double>(gyro->center) + gyro->offset)) *
1e-9 * HAL_GetAnalogLSBWeight(gyro->handle, status) /
((1 << HAL_GetAnalogOversampleBits(gyro->handle, status)) *
gyro->voltsPerDegreePerSecond);
}
double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return gyro->offset;
}
int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status) {
auto gyro = analogGyroHandles->Get(handle);
if (gyro == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return gyro->center;
}
} // extern "C"

274
hal/src/main/native/athena/AnalogInput.cpp
View File

@@ -1,274 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/AnalogInput.h"
#include <FRC_NetworkCommunication/AICalibration.h>
#include <wpi/mutex.h>
#include "AnalogInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/AnalogAccumulator.h"
#include "hal/handles/HandlesInternal.h"
namespace hal::init {
void InitializeAnalogInput() {}
} // namespace hal::init
using namespace hal;
extern "C" {
HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(
HAL_PortHandle portHandle, const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
initializeAnalog(status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
int16_t channel = getPortHandleChannel(portHandle);
if (channel == InvalidHandleIndex || channel >= kNumAnalogInputs) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
0, kNumAnalogInputs, channel);
return HAL_kInvalidHandle;
}
HAL_AnalogInputHandle handle;
auto analog_port = analogInputHandles->Allocate(channel, &handle, status);
if (*status != 0) {
if (analog_port) {
hal::SetLastErrorPreviouslyAllocated(status, "Analog Input", channel,
analog_port->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
0, kNumAnalogInputs, channel);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
// Initialize port structure
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);
analog_port->previousAllocation =
allocationLocation ? allocationLocation : "";
return handle;
}
void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {
// no status, so no need to check for a proper free.
analogInputHandles->Free(analogPortHandle);
}
HAL_Bool HAL_CheckAnalogModule(int32_t module) {
return module == 1;
}
HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) {
return channel < kNumAnalogInputs && channel >= 0;
}
void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle,
HAL_SimDeviceHandle device) {}
void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status) {
// TODO: This will change when variable size scan lists are implemented.
// TODO: Need double comparison with epsilon.
// wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
initializeAnalog(status);
if (*status != 0) {
return;
}
setAnalogSampleRate(samplesPerSecond, status);
}
double HAL_GetAnalogSampleRate(int32_t* status) {
initializeAnalog(status);
if (*status != 0) {
return 0;
}
uint32_t ticksPerConversion = analogInputSystem->readLoopTiming(status);
uint32_t ticksPerSample =
ticksPerConversion * getAnalogNumActiveChannels(status);
return static_cast<double>(kTimebase) / static_cast<double>(ticksPerSample);
}
void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
int32_t bits, int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
analogInputSystem->writeAverageBits(port->channel, static_cast<uint8_t>(bits),
status);
}
int32_t HAL_GetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return kDefaultAverageBits;
}
uint8_t result = analogInputSystem->readAverageBits(port->channel, status);
return result;
}
void HAL_SetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
int32_t bits, int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
analogInputSystem->writeOversampleBits(port->channel,
static_cast<uint8_t>(bits), status);
}
int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return kDefaultOversampleBits;
}
uint8_t result = analogInputSystem->readOversampleBits(port->channel, status);
return result;
}
int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
tAI::tReadSelect readSelect;
readSelect.Channel = port->channel;
readSelect.Averaged = false;
std::scoped_lock lock(analogRegisterWindowMutex);
analogInputSystem->writeReadSelect(readSelect, status);
analogInputSystem->strobeLatchOutput(status);
return static_cast<int16_t>(analogInputSystem->readOutput(status));
}
int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
auto port = analogInputHandles->Get(analogPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
tAI::tReadSelect readSelect;
readSelect.Channel = port->channel;
readSelect.Averaged = true;
std::scoped_lock lock(analogRegisterWindowMutex);
analogInputSystem->writeReadSelect(readSelect, status);
analogInputSystem->strobeLatchOutput(status);
return static_cast<int32_t>(analogInputSystem->readOutput(status));
}
int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle,
double voltage, int32_t* status) {
if (voltage > 5.0) {
voltage = 5.0;
*status = VOLTAGE_OUT_OF_RANGE;
}
if (voltage < 0.0) {
voltage = 0.0;
*status = VOLTAGE_OUT_OF_RANGE;
}
int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
int32_t value =
static_cast<int32_t>((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9));
return value;
}
double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
int32_t value = HAL_GetAnalogValue(analogPortHandle, status);
int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
double voltage = LSBWeight * 1.0e-9 * value - offset * 1.0e-9;
return voltage;
}
double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
int32_t rawValue, int32_t* status) {
int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
double voltage = LSBWeight * 1.0e-9 * rawValue - offset * 1.0e-9;
return voltage;
}
double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
int32_t value = HAL_GetAnalogAverageValue(analogPortHandle, status);
int32_t LSBWeight = HAL_GetAnalogLSBWeight(analogPortHandle, status);
int32_t offset = HAL_GetAnalogOffset(analogPortHandle, status);
int32_t oversampleBits =
HAL_GetAnalogOversampleBits(analogPortHandle, status);
double voltage =
LSBWeight * 1.0e-9 * value / static_cast<double>(1 << oversampleBits) -
offset * 1.0e-9;
return voltage;
}
int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
// On the roboRIO, LSB is the same for all channels. So the channel lookup can
// be avoided
return FRC_NetworkCommunication_nAICalibration_getLSBWeight(0, 0, status);
// Keep the old code for future hardware
// auto port = analogInputHandles->Get(analogPortHandle);
// if (port == nullptr) {
// *status = HAL_HANDLE_ERROR;
// return 0;
// }
// int32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(
// 0, port->channel, status); // XXX: aiSystemIndex == 0?
// return lsbWeight;
}
int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
int32_t* status) {
// On the roboRIO, offset is the same for all channels. So the channel lookup
// can be avoided
return FRC_NetworkCommunication_nAICalibration_getOffset(0, 0, status);
// Keep the old code for future hardware
// auto port = analogInputHandles->Get(analogPortHandle);
// if (port == nullptr) {
// *status = HAL_HANDLE_ERROR;
// return 0;
// }
// int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(
// 0, port->channel, status); // XXX: aiSystemIndex == 0?
// return offset;
}
} // extern "C"

107
hal/src/main/native/athena/AnalogInternal.cpp
View File

@@ -1,107 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "AnalogInternal.h"
#include <atomic>
#include <memory>
#include <wpi/mutex.h>
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/AnalogInput.h"
#include "hal/ChipObject.h"
namespace hal {
wpi::mutex analogRegisterWindowMutex;
std::unique_ptr<tAI> analogInputSystem;
std::unique_ptr<tAO> analogOutputSystem;
IndexedHandleResource<HAL_AnalogInputHandle, ::hal::AnalogPort,
kNumAnalogInputs, HAL_HandleEnum::AnalogInput>*
analogInputHandles;
static int32_t analogNumChannelsToActivate = 0;
static std::atomic<bool> analogSystemInitialized{false};
bool analogSampleRateSet = false;
namespace init {
void InitializeAnalogInternal() {
static IndexedHandleResource<HAL_AnalogInputHandle, ::hal::AnalogPort,
kNumAnalogInputs, HAL_HandleEnum::AnalogInput>
alH;
analogInputHandles = &alH;
}
} // namespace init
void initializeAnalog(int32_t* status) {
hal::init::CheckInit();
if (analogSystemInitialized) {
return;
}
std::scoped_lock lock(analogRegisterWindowMutex);
if (analogSystemInitialized) {
return;
}
analogInputSystem.reset(tAI::create(status));
analogOutputSystem.reset(tAO::create(status));
setAnalogNumChannelsToActivate(kNumAnalogInputs);
setAnalogSampleRate(kDefaultSampleRate, status);
analogSystemInitialized = true;
}
int32_t getAnalogNumActiveChannels(int32_t* status) {
int32_t scanSize = analogInputSystem->readConfig_ScanSize(status);
if (scanSize == 0) {
return 8;
}
return scanSize;
}
void setAnalogNumChannelsToActivate(int32_t channels) {
analogNumChannelsToActivate = channels;
}
int32_t getAnalogNumChannelsToActivate(int32_t* status) {
if (analogNumChannelsToActivate == 0) {
return getAnalogNumActiveChannels(status);
}
return analogNumChannelsToActivate;
}
void setAnalogSampleRate(double samplesPerSecond, int32_t* status) {
// TODO: This will change when variable size scan lists are implemented.
// TODO: Need double comparison with epsilon.
// wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
analogSampleRateSet = true;
// Compute the convert rate
uint32_t ticksPerSample =
static_cast<uint32_t>(static_cast<double>(kTimebase) / samplesPerSecond);
uint32_t ticksPerConversion =
ticksPerSample / getAnalogNumChannelsToActivate(status);
// ticksPerConversion must be at least 80
if (ticksPerConversion < 80) {
if ((*status) >= 0) {
*status = SAMPLE_RATE_TOO_HIGH;
}
ticksPerConversion = 80;
}
// Atomically set the scan size and the convert rate so that the sample rate
// is constant
tAI::tConfig config;
config.ScanSize = getAnalogNumChannelsToActivate(status);
config.ConvertRate = ticksPerConversion;
analogInputSystem->writeConfig(config, status);
// Indicate that the scan size has been committed to hardware.
setAnalogNumChannelsToActivate(0);
}
} // namespace hal

87
hal/src/main/native/athena/AnalogInternal.h
View File

@@ -1,87 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
#include <memory>
#include <string>
#include <wpi/mutex.h>
#include "PortsInternal.h"
#include "hal/ChipObject.h"
#include "hal/Ports.h"
#include "hal/handles/IndexedHandleResource.h"
namespace hal {
constexpr int32_t kTimebase = 40000000; ///< 40 MHz clock
constexpr int32_t kDefaultOversampleBits = 0;
constexpr int32_t kDefaultAverageBits = 7;
constexpr double kDefaultSampleRate = 50000.0;
static constexpr uint32_t kAccumulatorChannels[] = {0, 1};
extern std::unique_ptr<tAI> analogInputSystem;
extern std::unique_ptr<tAO> analogOutputSystem;
extern wpi::mutex analogRegisterWindowMutex;
extern bool analogSampleRateSet;
struct AnalogPort {
uint8_t channel;
std::unique_ptr<tAccumulator> accumulator;
std::string previousAllocation;
};
extern IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
kNumAnalogInputs, HAL_HandleEnum::AnalogInput>*
analogInputHandles;
/**
* Initialize the analog System.
*/
void initializeAnalog(int32_t* status);
/**
* Return the number of channels on the module in use.
*
* @return Active channels.
*/
int32_t getAnalogNumActiveChannels(int32_t* status);
/**
* Set the number of active channels.
*
* Store the number of active channels to set. Don't actually commit to
* hardware
* until SetSampleRate().
*
* @param channels Number of active channels.
*/
void setAnalogNumChannelsToActivate(int32_t channels);
/**
* Get the number of active channels.
*
* This is an internal function to allow the atomic update of both the
* number of active channels and the sample rate.
*
* When the number of channels changes, use the new value. Otherwise,
* return the current value.
*
* @return Value to write to the active channels field.
*/
int32_t getAnalogNumChannelsToActivate(int32_t* status);
/**
* Set the sample rate.
*
* This is a global setting for the Athena and effects all channels.
*
* @param samplesPerSecond The number of samples per channel per second.
*/
void setAnalogSampleRate(double samplesPerSecond, int32_t* status);
} // namespace hal

123
hal/src/main/native/athena/AnalogOutput.cpp
View File

@@ -1,123 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/AnalogOutput.h"
#include <string>
#include "AnalogInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/Errors.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/IndexedHandleResource.h"
using namespace hal;
namespace {
struct AnalogOutput {
uint8_t channel;
std::string previousAllocation;
};
} // namespace
static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>*
analogOutputHandles;
namespace hal::init {
void InitializeAnalogOutput() {
static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>
aoH;
analogOutputHandles = &aoH;
}
} // namespace hal::init
extern "C" {
HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(
HAL_PortHandle portHandle, const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
initializeAnalog(status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
int16_t channel = getPortHandleChannel(portHandle);
if (channel == InvalidHandleIndex || channel >= kNumAnalogOutputs) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Output",
0, kNumAnalogOutputs, channel);
return HAL_kInvalidHandle;
}
HAL_AnalogOutputHandle handle;
auto port = analogOutputHandles->Allocate(channel, &handle, status);
if (*status != 0) {
if (port) {
hal::SetLastErrorPreviouslyAllocated(status, "Analog Output", channel,
port->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status,
"Invalid Index for Analog Output", 0,
kNumAnalogOutputs, channel);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
port->channel = static_cast<uint8_t>(channel);
port->previousAllocation = allocationLocation ? allocationLocation : "";
return handle;
}
void HAL_FreeAnalogOutputPort(HAL_AnalogOutputHandle analogOutputHandle) {
// no status, so no need to check for a proper free.
analogOutputHandles->Free(analogOutputHandle);
}
void HAL_SetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
double voltage, int32_t* status) {
auto port = analogOutputHandles->Get(analogOutputHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint16_t rawValue = static_cast<uint16_t>(voltage / 5.0 * 0x1000);
if (voltage < 0.0) {
rawValue = 0;
} else if (voltage > 5.0) {
rawValue = 0x1000;
}
analogOutputSystem->writeMXP(port->channel, rawValue, status);
}
double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
int32_t* status) {
auto port = analogOutputHandles->Get(analogOutputHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0.0;
}
uint16_t rawValue = analogOutputSystem->readMXP(port->channel, status);
return rawValue * 5.0 / 0x1000;
}
HAL_Bool HAL_CheckAnalogOutputChannel(int32_t channel) {
return channel < kNumAnalogOutputs && channel >= 0;
}
} // extern "C"

272
hal/src/main/native/athena/AnalogTrigger.cpp
View File

@@ -1,272 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/AnalogTrigger.h"
#include <memory>
#include "AnalogInternal.h"
#include "ConstantsInternal.h"
#include "DutyCycleInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/AnalogInput.h"
#include "hal/DutyCycle.h"
#include "hal/Errors.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
using namespace hal;
namespace {
struct AnalogTrigger {
std::unique_ptr<tAnalogTrigger> trigger;
HAL_Handle handle;
uint8_t index;
};
} // namespace
static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
kNumAnalogTriggers, HAL_HandleEnum::AnalogTrigger>*
analogTriggerHandles;
namespace hal::init {
void InitializeAnalogTrigger() {
static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
kNumAnalogTriggers,
HAL_HandleEnum::AnalogTrigger>
atH;
analogTriggerHandles = &atH;
}
} // namespace hal::init
extern "C" {
HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
HAL_AnalogInputHandle portHandle, int32_t* status) {
hal::init::CheckInit();
// ensure we are given a valid and active AnalogInput handle
auto analog_port = analogInputHandles->Get(portHandle);
if (analog_port == nullptr) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto trigger = analogTriggerHandles->Get(handle);
if (trigger == nullptr) { // would only occur on thread issue
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
trigger->handle = portHandle;
trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
trigger->trigger->writeSourceSelect_Channel(analog_port->channel, status);
return handle;
}
HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
hal::init::CheckInit();
// ensure we are given a valid and active DutyCycle handle
auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
if (dutyCycle == nullptr) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
HAL_AnalogTriggerHandle handle = analogTriggerHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto trigger = analogTriggerHandles->Get(handle);
if (trigger == nullptr) { // would only occur on thread issue
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
trigger->handle = dutyCycleHandle;
trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
trigger->trigger.reset(tAnalogTrigger::create(trigger->index, status));
trigger->trigger->writeSourceSelect_Channel(dutyCycle->index, status);
trigger->trigger->writeSourceSelect_DutyCycle(true, status);
return handle;
}
void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle) {
analogTriggerHandles->Free(analogTriggerHandle);
// caller owns the input handle.
}
void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
int32_t lower, int32_t upper,
int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (lower > upper) {
*status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
return;
}
trigger->trigger->writeLowerLimit(lower, status);
trigger->trigger->writeUpperLimit(upper, status);
}
void HAL_SetAnalogTriggerLimitsDutyCycle(
HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (getHandleType(trigger->handle) != HAL_HandleEnum::DutyCycle) {
*status = HAL_HANDLE_ERROR;
return;
}
if (lower > upper) {
*status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
return;
}
if (lower < 0.0 || upper > 1.0) {
*status = PARAMETER_OUT_OF_RANGE;
auto lowerStr = std::to_string(lower);
auto upperStr = std::to_string(upper);
hal::SetLastError(
status, "Lower must be >= 0 and upper must be <=1. Requested lower " +
lowerStr + " Requested upper " + upperStr);
return;
}
trigger->trigger->writeLowerLimit(
static_cast<int32_t>(kDutyCycleScaleFactor * lower), status);
trigger->trigger->writeUpperLimit(
static_cast<int32_t>(kDutyCycleScaleFactor * upper), status);
}
void HAL_SetAnalogTriggerLimitsVoltage(
HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (getHandleType(trigger->handle) != HAL_HandleEnum::AnalogInput) {
*status = HAL_HANDLE_ERROR;
return;
}
if (lower > upper) {
*status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR;
return;
}
// TODO: This depends on the averaged setting. Only raw values will work as
// is.
trigger->trigger->writeLowerLimit(
HAL_GetAnalogVoltsToValue(trigger->handle, lower, status), status);
trigger->trigger->writeUpperLimit(
HAL_GetAnalogVoltsToValue(trigger->handle, upper, status), status);
}
void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
HAL_Bool useAveragedValue, int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (trigger->trigger->readSourceSelect_Filter(status) != 0 ||
trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
*status = INCOMPATIBLE_STATE;
// TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not
// support average and filtering at the same time.");
}
trigger->trigger->writeSourceSelect_Averaged(useAveragedValue, status);
}
void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
HAL_Bool useFilteredValue, int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (trigger->trigger->readSourceSelect_Averaged(status) != 0 ||
trigger->trigger->readSourceSelect_DutyCycle(status) != 0) {
*status = INCOMPATIBLE_STATE;
// TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not "
// "support average and filtering at the same time.");
}
trigger->trigger->writeSourceSelect_Filter(useFilteredValue, status);
}
HAL_Bool HAL_GetAnalogTriggerInWindow(
HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
return trigger->trigger->readOutput_InHysteresis(trigger->index, status) != 0;
}
HAL_Bool HAL_GetAnalogTriggerTriggerState(
HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
return trigger->trigger->readOutput_OverLimit(trigger->index, status) != 0;
}
HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
HAL_AnalogTriggerType type,
int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
bool result = false;
switch (type) {
case HAL_Trigger_kInWindow:
result =
trigger->trigger->readOutput_InHysteresis(trigger->index, status);
break;
case HAL_Trigger_kState:
result = trigger->trigger->readOutput_OverLimit(trigger->index, status);
break;
case HAL_Trigger_kRisingPulse:
case HAL_Trigger_kFallingPulse:
*status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR;
return false;
break;
}
return result;
}
int32_t HAL_GetAnalogTriggerFPGAIndex(
HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status) {
auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
if (trigger == nullptr) {
*status = HAL_HANDLE_ERROR;
return -1;
}
return trigger->index;
}
} // extern "C"

50
hal/src/main/native/athena/CAN.cpp
View File

@@ -1,50 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/CAN.h"
#include <FRC_NetworkCommunication/CANSessionMux.h>
namespace hal::init {
void InitializeCAN() {}
} // namespace hal::init
extern "C" {
void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
uint8_t dataSize, int32_t periodMs, int32_t* status) {
FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, data, dataSize,
periodMs, status);
}
void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
uint8_t* data, uint8_t* dataSize,
uint32_t* timeStamp, int32_t* status) {
FRC_NetworkCommunication_CANSessionMux_receiveMessage(
messageID, messageIDMask, data, dataSize, timeStamp, status);
}
void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
uint32_t messageIDMask, uint32_t maxMessages,
int32_t* status) {
FRC_NetworkCommunication_CANSessionMux_openStreamSession(
sessionHandle, messageID, messageIDMask, maxMessages, status);
}
void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {
FRC_NetworkCommunication_CANSessionMux_closeStreamSession(sessionHandle);
}
void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
struct HAL_CANStreamMessage* messages,
uint32_t messagesToRead, uint32_t* messagesRead,
int32_t* status) {
FRC_NetworkCommunication_CANSessionMux_readStreamSession(
sessionHandle, reinterpret_cast<tCANStreamMessage*>(messages),
messagesToRead, messagesRead, status);
}
void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
uint32_t* txFullCount, uint32_t* receiveErrorCount,
uint32_t* transmitErrorCount, int32_t* status) {
FRC_NetworkCommunication_CANSessionMux_getCANStatus(
percentBusUtilization, busOffCount, txFullCount, receiveErrorCount,
transmitErrorCount, status);
}
} // extern "C"

286
hal/src/main/native/athena/CANAPI.cpp
View File

@@ -1,286 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/CANAPI.h"
#include <ctime>
#include <memory>
#include <wpi/DenseMap.h>
#include <wpi/mutex.h>
#include "HALInitializer.h"
#include "hal/CAN.h"
#include "hal/Errors.h"
#include "hal/handles/UnlimitedHandleResource.h"
using namespace hal;
namespace {
struct Receives {
uint32_t lastTimeStamp;
uint8_t data[8];
uint8_t length;
};
struct CANStorage {
HAL_CANManufacturer manufacturer;
HAL_CANDeviceType deviceType;
uint8_t deviceId;
wpi::mutex periodicSendsMutex;
wpi::SmallDenseMap<int32_t, int32_t> periodicSends;
wpi::mutex receivesMutex;
wpi::SmallDenseMap<int32_t, Receives> receives;
};
} // namespace
static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>*
canHandles;
namespace hal::init {
void InitializeCANAPI() {
static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>
cH;
canHandles = &cH;
}
} // namespace hal::init
static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
int32_t createdId = 0;
createdId |= (static_cast<int32_t>(storage->deviceType) & 0x1F) << 24;
createdId |= (static_cast<int32_t>(storage->manufacturer) & 0xFF) << 16;
createdId |= (apiId & 0x3FF) << 6;
createdId |= (storage->deviceId & 0x3F);
return createdId;
}
extern "C" {
uint32_t HAL_GetCANPacketBaseTime(void) {
timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
// Convert t to milliseconds
uint64_t ms = t.tv_sec * 1000ull + t.tv_nsec / 1000000ull;
return ms & 0xFFFFFFFF;
}
HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
int32_t deviceId, HAL_CANDeviceType deviceType,
int32_t* status) {
hal::init::CheckInit();
auto can = std::make_shared<CANStorage>();
auto handle = canHandles->Allocate(can);
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
can->deviceId = deviceId;
can->deviceType = deviceType;
can->manufacturer = manufacturer;
return handle;
}
void HAL_CleanCAN(HAL_CANHandle handle) {
auto data = canHandles->Free(handle);
if (data == nullptr) {
return;
}
std::scoped_lock lock(data->periodicSendsMutex);
for (auto&& i : data->periodicSends) {
int32_t s = 0;
auto id = CreateCANId(data.get(), i.first);
HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, &s);
i.second = -1;
}
}
void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
int32_t length, int32_t apiId, int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
auto id = CreateCANId(can.get(), apiId);
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
can->periodicSends[apiId] = -1;
}
void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
int32_t length, int32_t apiId,
int32_t repeatMs, int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
auto id = CreateCANId(can.get(), apiId);
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, data, length, repeatMs, status);
can->periodicSends[apiId] = repeatMs;
}
void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
auto id = CreateCANId(can.get(), apiId);
id |= HAL_CAN_IS_FRAME_REMOTE;
uint8_t data[8];
std::memset(data, 0, sizeof(data));
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
can->periodicSends[apiId] = -1;
}
void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
auto id = CreateCANId(can.get(), apiId);
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
status);
can->periodicSends[apiId] = -1;
}
void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
int32_t* length, uint64_t* receivedTimestamp,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint8_t dataSize = 0;
uint32_t ts = 0;
HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
if (*status == 0) {
std::scoped_lock lock(can->receivesMutex);
auto& msg = can->receives[messageId];
msg.length = dataSize;
msg.lastTimeStamp = ts;
// The NetComm call placed in data, copy into the msg
std::memcpy(msg.data, data, dataSize);
}
*length = dataSize;
*receivedTimestamp = ts;
}
void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
int32_t* length, uint64_t* receivedTimestamp,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint8_t dataSize = 0;
uint32_t ts = 0;
HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
std::scoped_lock lock(can->receivesMutex);
if (*status == 0) {
// fresh update
auto& msg = can->receives[messageId];
msg.length = dataSize;
*length = dataSize;
msg.lastTimeStamp = ts;
*receivedTimestamp = ts;
// The NetComm call placed in data, copy into the msg
std::memcpy(msg.data, data, dataSize);
} else {
auto i = can->receives.find(messageId);
if (i != can->receives.end()) {
// Read the data from the stored message into the output
std::memcpy(data, i->second.data, i->second.length);
*length = i->second.length;
*receivedTimestamp = i->second.lastTimeStamp;
*status = 0;
}
}
}
void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
uint8_t* data, int32_t* length,
uint64_t* receivedTimestamp, int32_t timeoutMs,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return;
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint8_t dataSize = 0;
uint32_t ts = 0;
HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
std::scoped_lock lock(can->receivesMutex);
if (*status == 0) {
// fresh update
auto& msg = can->receives[messageId];
msg.length = dataSize;
*length = dataSize;
msg.lastTimeStamp = ts;
*receivedTimestamp = ts;
// The NetComm call placed in data, copy into the msg
std::memcpy(msg.data, data, dataSize);
} else {
auto i = can->receives.find(messageId);
if (i != can->receives.end()) {
// Found, check if new enough
uint32_t now = HAL_GetCANPacketBaseTime();
if (now - i->second.lastTimeStamp > static_cast<uint32_t>(timeoutMs)) {
// Timeout, return bad status
*status = HAL_CAN_TIMEOUT;
return;
}
// Read the data from the stored message into the output
std::memcpy(data, i->second.data, i->second.length);
*length = i->second.length;
*receivedTimestamp = i->second.lastTimeStamp;
*status = 0;
}
}
}
uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return 0;
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint32_t session = 0;
HAL_CAN_OpenStreamSession(&session, messageId, 0x1FFFFFFF, depth, status);
return session;
}
} // extern "C"

407
hal/src/main/native/athena/CTREPCM.cpp
View File

@@ -1,407 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/CTREPCM.h"
#include <string>
#include <fmt/format.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/CANAPI.h"
#include "hal/Errors.h"
#include "hal/handles/IndexedHandleResource.h"
using namespace hal;
static constexpr HAL_CANManufacturer manufacturer =
HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
static constexpr HAL_CANDeviceType deviceType =
HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics;
static constexpr int32_t Status1 = 0x50;
static constexpr int32_t StatusSolFaults = 0x51;
static constexpr int32_t StatusDebug = 0x52;
static constexpr int32_t Control1 = 0x70;
static constexpr int32_t Control2 = 0x71;
static constexpr int32_t Control3 = 0x72;
static constexpr int32_t TimeoutMs = 100;
static constexpr int32_t SendPeriod = 20;
union PcmStatus {
uint8_t data[8];
struct Bits {
/* 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;
} bits;
};
union PcmControl {
uint8_t data[8];
struct Bits {
/* 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;
} bits;
};
struct PcmControlSetOneShotDur {
uint8_t sol10MsPerUnit[8];
};
union PcmStatusFault {
uint8_t data[8];
struct Bits {
/* Byte 0 */
unsigned SolenoidDisabledList : 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;
} bits;
};
union PcmDebug {
uint8_t data[8];
struct Bits {
unsigned tokFailsTop8 : 8;
unsigned tokFailsBtm8 : 8;
unsigned lastFailedTokTop8 : 8;
unsigned lastFailedTokBtm8 : 8;
unsigned tokSuccessTop8 : 8;
unsigned tokSuccessBtm8 : 8;
} bits;
};
namespace {
struct PCM {
HAL_CANHandle canHandle;
wpi::mutex lock;
std::string previousAllocation;
PcmControl control;
PcmControlSetOneShotDur oneShot;
};
} // namespace
static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
HAL_HandleEnum::CTREPCM>* pcmHandles;
namespace hal::init {
void InitializeCTREPCM() {
static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
HAL_HandleEnum::CTREPCM>
pH;
pcmHandles = &pH;
}
} // namespace hal::init
#define READ_PACKET(type, frame, failureValue) \
auto pcm = pcmHandles->Get(handle); \
if (pcm == nullptr) { \
*status = HAL_HANDLE_ERROR; \
return failureValue; \
} \
type pcmStatus; \
int32_t length = 0; \
uint64_t receivedTimestamp = 0; \
HAL_ReadCANPacketTimeout(pcm->canHandle, frame, pcmStatus.data, &length, \
&receivedTimestamp, TimeoutMs, status); \
if (*status != 0) { \
return failureValue; \
}
#define READ_STATUS(failureValue) READ_PACKET(PcmStatus, Status1, failureValue)
#define READ_SOL_FAULTS(failureValue) \
READ_PACKET(PcmStatusFault, StatusSolFaults, failureValue)
static void SendControl(PCM* pcm, int32_t* status) {
HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->control.data, 8, Control1,
SendPeriod, status);
}
extern "C" {
HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
HAL_CTREPCMHandle handle;
auto pcm = pcmHandles->Allocate(module, &handle, status);
if (*status != 0) {
if (pcm) {
hal::SetLastErrorPreviouslyAllocated(status, "CTRE PCM", module,
pcm->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0,
kNumCTREPCMModules - 1, module);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
pcm->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
if (*status != 0) {
pcmHandles->Free(handle);
return HAL_kInvalidHandle;
}
std::memset(&pcm->oneShot, 0, sizeof(pcm->oneShot));
std::memset(&pcm->control, 0, sizeof(pcm->control));
pcm->previousAllocation = allocationLocation ? allocationLocation : "";
// Enable closed loop control
HAL_SetCTREPCMClosedLoopControl(handle, true, status);
if (*status != 0) {
HAL_FreeCTREPCM(handle);
return HAL_kInvalidHandle;
}
return handle;
}
void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle) {
auto pcm = pcmHandles->Get(handle);
if (pcm) {
HAL_CleanCAN(pcm->canHandle);
}
pcmHandles->Free(handle);
}
HAL_Bool HAL_CheckCTREPCMSolenoidChannel(int32_t channel) {
return channel < kNumCTRESolenoidChannels && channel >= 0;
}
HAL_Bool HAL_GetCTREPCMCompressor(HAL_CTREPCMHandle handle, int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.compressorOn;
}
void HAL_SetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, HAL_Bool enabled,
int32_t* status) {
auto pcm = pcmHandles->Get(handle);
if (pcm == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t can_status = 0;
std::scoped_lock lock{pcm->lock};
pcm->control.bits.closedLoopEnable = enabled ? 1 : 0;
SendControl(pcm.get(), &can_status);
}
HAL_Bool HAL_GetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.isCloseloopEnabled;
}
HAL_Bool HAL_GetCTREPCMPressureSwitch(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.pressureSwitchEn;
}
double HAL_GetCTREPCMCompressorCurrent(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(0);
uint32_t result = pcmStatus.bits.compressorCurrentTop6;
result <<= 4;
result |= pcmStatus.bits.compressorCurrentBtm4;
return result * 0.03125; /* 5.5 fixed pt value in Amps */
}
HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighFault(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.faultCompCurrentTooHigh;
}
HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(
HAL_CTREPCMHandle handle, int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.stickyFaultCompCurrentTooHigh;
}
HAL_Bool HAL_GetCTREPCMCompressorShortedStickyFault(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.Fault_dItooHigh;
}
HAL_Bool HAL_GetCTREPCMCompressorShortedFault(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.StickyFault_dItooHigh;
}
HAL_Bool HAL_GetCTREPCMCompressorNotConnectedStickyFault(
HAL_CTREPCMHandle handle, int32_t* status) {
READ_SOL_FAULTS(false);
return pcmStatus.bits.StickyFault_CompNoCurrent;
}
HAL_Bool HAL_GetCTREPCMCompressorNotConnectedFault(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_SOL_FAULTS(false);
return pcmStatus.bits.Fault_CompNoCurrent;
}
int32_t HAL_GetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t* status) {
READ_STATUS(0);
return pcmStatus.bits.SolenoidBits & 0xFF;
}
void HAL_SetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t mask,
int32_t values, int32_t* status) {
auto pcm = pcmHandles->Get(handle);
if (pcm == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t smallMask = mask & 0xFF;
uint8_t smallValues =
(values & 0xFF) & smallMask; // Enforce only masked values are set
uint8_t invertMask = ~smallMask;
std::scoped_lock lock{pcm->lock};
uint8_t existingValue = invertMask & pcm->control.bits.solenoidBits;
pcm->control.bits.solenoidBits = existingValue | smallValues;
SendControl(pcm.get(), status);
}
int32_t HAL_GetCTREPCMSolenoidDisabledList(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_SOL_FAULTS(0);
return pcmStatus.bits.SolenoidDisabledList;
}
HAL_Bool HAL_GetCTREPCMSolenoidVoltageStickyFault(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.stickyFaultFuseTripped;
}
HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle,
int32_t* status) {
READ_STATUS(false);
return pcmStatus.bits.faultFuseTripped;
}
void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle,
int32_t* status) {
auto pcm = pcmHandles->Get(handle);
if (pcm == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t controlData[] = {0, 0, 0, 0x80};
HAL_WriteCANPacket(pcm->canHandle, controlData, sizeof(controlData), Control2,
status);
}
void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index,
int32_t* status) {
if (index > 7 || index < 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Only [0-7] are valid index values. Requested {}", index));
return;
}
auto pcm = pcmHandles->Get(handle);
if (pcm == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
std::scoped_lock lock{pcm->lock};
uint16_t oneShotField = pcm->control.bits.OneShotField_h8;
oneShotField <<= 8;
oneShotField |= pcm->control.bits.OneShotField_l8;
uint16_t shift = 2 * index;
uint16_t mask = 3;
uint8_t chBits = (oneShotField >> shift) & mask;
chBits = (chBits % 3) + 1;
oneShotField &= ~(mask << shift);
oneShotField |= (chBits << shift);
pcm->control.bits.OneShotField_h8 = oneShotField >> 8;
pcm->control.bits.OneShotField_l8 = oneShotField;
SendControl(pcm.get(), status);
}
void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
int32_t durMs, int32_t* status) {
if (index > 7 || index < 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Only [0-7] are valid index values. Requested {}", index));
return;
}
auto pcm = pcmHandles->Get(handle);
if (pcm == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
std::scoped_lock lock{pcm->lock};
pcm->oneShot.sol10MsPerUnit[index] = (std::min)(
static_cast<uint32_t>(durMs) / 10, static_cast<uint32_t>(0xFF));
HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->oneShot.sol10MsPerUnit, 8,
Control3, SendPeriod, status);
}
} // extern "C"

769
hal/src/main/native/athena/CTREPDP.cpp
View File

@@ -1,769 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "CTREPDP.h"
#include <string>
#include <fmt/format.h>
#include <wpi/mutex.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/CAN.h"
#include "hal/CANAPI.h"
#include "hal/Errors.h"
#include "hal/handles/IndexedHandleResource.h"
using namespace hal;
static constexpr HAL_CANManufacturer manufacturer =
HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
static constexpr HAL_CANDeviceType deviceType =
HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
static constexpr int32_t Status1 = 0x50;
static constexpr int32_t Status2 = 0x51;
static constexpr int32_t Status3 = 0x52;
static constexpr int32_t StatusEnergy = 0x5D;
static constexpr int32_t Control1 = 0x70;
static constexpr int32_t TimeoutMs = 100;
/* encoder/decoders */
union PdpStatus1 {
uint8_t data[8];
struct Bits {
unsigned chan1_h8 : 8;
unsigned chan2_h6 : 6;
unsigned chan1_l2 : 2;
unsigned chan3_h4 : 4;
unsigned chan2_l4 : 4;
unsigned chan4_h2 : 2;
unsigned chan3_l6 : 6;
unsigned chan4_l8 : 8;
unsigned chan5_h8 : 8;
unsigned chan6_h6 : 6;
unsigned chan5_l2 : 2;
unsigned reserved4 : 4;
unsigned chan6_l4 : 4;
} bits;
};
union PdpStatus2 {
uint8_t data[8];
struct Bits {
unsigned chan7_h8 : 8;
unsigned chan8_h6 : 6;
unsigned chan7_l2 : 2;
unsigned chan9_h4 : 4;
unsigned chan8_l4 : 4;
unsigned chan10_h2 : 2;
unsigned chan9_l6 : 6;
unsigned chan10_l8 : 8;
unsigned chan11_h8 : 8;
unsigned chan12_h6 : 6;
unsigned chan11_l2 : 2;
unsigned reserved4 : 4;
unsigned chan12_l4 : 4;
} bits;
};
union PdpStatus3 {
uint8_t data[8];
struct Bits {
unsigned chan13_h8 : 8;
unsigned chan14_h6 : 6;
unsigned chan13_l2 : 2;
unsigned chan15_h4 : 4;
unsigned chan14_l4 : 4;
unsigned chan16_h2 : 2;
unsigned chan15_l6 : 6;
unsigned chan16_l8 : 8;
unsigned internalResBattery_mOhms : 8;
unsigned busVoltage : 8;
unsigned temp : 8;
} bits;
};
union PdpStatusEnergy {
uint8_t data[8];
struct Bits {
unsigned TmeasMs_likelywillbe20ms_ : 8;
unsigned TotalCurrent_125mAperunit_h8 : 8;
unsigned Power_125mWperunit_h4 : 4;
unsigned TotalCurrent_125mAperunit_l4 : 4;
unsigned Power_125mWperunit_m8 : 8;
unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
unsigned Power_125mWperunit_l4 : 4;
unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
} bits;
};
namespace {
struct PDP {
HAL_CANHandle canHandle;
std::string previousAllocation;
bool streamHandleAllocated{false};
uint32_t streamSessionHandles[3];
};
} // namespace
static IndexedHandleResource<HAL_PDPHandle, PDP, kNumCTREPDPModules,
HAL_HandleEnum::CTREPDP>* pdpHandles;
namespace hal::init {
void InitializeCTREPDP() {
static IndexedHandleResource<HAL_PDPHandle, PDP, kNumCTREPDPModules,
HAL_HandleEnum::CTREPDP>
pH;
pdpHandles = &pH;
}
} // namespace hal::init
extern "C" {
HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
if (!HAL_CheckPDPModule(module)) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0,
kNumCTREPDPModules - 1, module);
return HAL_kInvalidHandle;
}
HAL_PDPHandle handle;
auto pdp = pdpHandles->Allocate(module, &handle, status);
if (*status != 0) {
if (pdp) {
hal::SetLastErrorPreviouslyAllocated(status, "CTRE PDP", module,
pdp->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0,
kNumCTREPDPModules - 1, module);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
pdp->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
if (*status != 0) {
pdpHandles->Free(handle);
return HAL_kInvalidHandle;
}
pdp->previousAllocation = allocationLocation ? allocationLocation : "";
return handle;
}
void HAL_CleanPDP(HAL_PDPHandle handle) {
auto pdp = pdpHandles->Get(handle);
if (pdp) {
HAL_CleanCAN(pdp->canHandle);
}
pdpHandles->Free(handle);
}
int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status) {
return hal::getHandleIndex(handle);
}
HAL_Bool HAL_CheckPDPModule(int32_t module) {
return module < kNumCTREPDPModules && module >= 0;
}
HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
return channel < kNumCTREPDPChannels && channel >= 0;
}
double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PdpStatus3 pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
} else {
return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
}
}
double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PdpStatus3 pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
} else {
return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
}
}
double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
int32_t* status) {
if (!HAL_CheckPDPChannel(channel)) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Invalid pdp channel {}", channel));
return 0;
}
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
int32_t length = 0;
uint64_t receivedTimestamp = 0;
double raw = 0;
if (channel <= 5) {
PdpStatus1 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
}
switch (channel) {
case 0:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
pdpStatus.bits.chan1_l2;
break;
case 1:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
pdpStatus.bits.chan2_l4;
break;
case 2:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
pdpStatus.bits.chan3_l6;
break;
case 3:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
pdpStatus.bits.chan4_l8;
break;
case 4:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
pdpStatus.bits.chan5_l2;
break;
case 5:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
pdpStatus.bits.chan6_l4;
break;
}
} else if (channel <= 11) {
PdpStatus2 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
}
switch (channel) {
case 6:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
pdpStatus.bits.chan7_l2;
break;
case 7:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
pdpStatus.bits.chan8_l4;
break;
case 8:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
pdpStatus.bits.chan9_l6;
break;
case 9:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
pdpStatus.bits.chan10_l8;
break;
case 10:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
pdpStatus.bits.chan11_l2;
break;
case 11:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
pdpStatus.bits.chan12_l4;
break;
}
} else {
PdpStatus3 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
}
switch (channel) {
case 12:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
pdpStatus.bits.chan13_l2;
break;
case 13:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
pdpStatus.bits.chan14_l4;
break;
case 14:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
pdpStatus.bits.chan15_l6;
break;
case 15:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
pdpStatus.bits.chan16_l8;
break;
}
}
/* convert to amps */
return raw * 0.125; /* 7.3 fixed pt value in Amps */
}
void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t length = 0;
uint64_t receivedTimestamp = 0;
PdpStatus1 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return;
}
PdpStatus2 pdpStatus2;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus2.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return;
}
PdpStatus3 pdpStatus3;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus3.data, &length,
&receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return;
}
currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
pdpStatus.bits.chan1_l2) *
0.125;
currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
pdpStatus.bits.chan2_l4) *
0.125;
currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
pdpStatus.bits.chan3_l6) *
0.125;
currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
pdpStatus.bits.chan4_l8) *
0.125;
currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
pdpStatus.bits.chan5_l2) *
0.125;
currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
pdpStatus.bits.chan6_l4) *
0.125;
currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
pdpStatus2.bits.chan7_l2) *
0.125;
currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
pdpStatus2.bits.chan8_l4) *
0.125;
currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
pdpStatus2.bits.chan9_l6) *
0.125;
currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
pdpStatus2.bits.chan10_l8) *
0.125;
currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
pdpStatus2.bits.chan11_l2) *
0.125;
currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
pdpStatus2.bits.chan12_l4) *
0.125;
currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
pdpStatus3.bits.chan13_l2) *
0.125;
currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
pdpStatus3.bits.chan14_l4) *
0.125;
currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
pdpStatus3.bits.chan15_l6) *
0.125;
currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
pdpStatus3.bits.chan16_l8) *
0.125;
}
double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PdpStatusEnergy pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
&length, &receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
}
uint32_t raw;
raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
raw <<= 4;
raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
return 0.125 * raw; /* 7.3 fixed pt value in Amps */
}
double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PdpStatusEnergy pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
&length, &receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
}
uint32_t raw;
raw = pdpStatus.bits.Power_125mWperunit_h4;
raw <<= 8;
raw |= pdpStatus.bits.Power_125mWperunit_m8;
raw <<= 4;
raw |= pdpStatus.bits.Power_125mWperunit_l4;
return 0.125 * raw; /* 7.3 fixed pt value in Watts */
}
double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PdpStatusEnergy pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
&length, &receivedTimestamp, TimeoutMs, status);
if (*status != 0) {
return 0;
}
uint32_t raw;
raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
raw <<= 8;
raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
raw <<= 8;
raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
raw <<= 8;
raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
energyJoules *= 0.001; /* convert from mW to W */
energyJoules *=
pdpStatus.bits
.TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
return energyJoules;
}
void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
}
void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
}
uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
int32_t* status);
void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (pdp->streamHandleAllocated) {
*status = RESOURCE_IS_ALLOCATED;
return;
}
pdp->streamSessionHandles[0] =
HAL_StartCANStream(pdp->canHandle, Status1, 50, status);
if (*status != 0) {
return;
}
pdp->streamSessionHandles[1] =
HAL_StartCANStream(pdp->canHandle, Status2, 50, status);
if (*status != 0) {
HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
return;
}
pdp->streamSessionHandles[2] =
HAL_StartCANStream(pdp->canHandle, Status3, 50, status);
if (*status != 0) {
HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]);
return;
}
pdp->streamHandleAllocated = true;
}
HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
int32_t* count,
int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return nullptr;
}
if (!pdp->streamHandleAllocated) {
*status = RESOURCE_OUT_OF_RANGE;
return nullptr;
}
*count = 0;
// 3 streams, 6 channels per stream, 50 depth per stream
HAL_PowerDistributionChannelData* retData =
new HAL_PowerDistributionChannelData[3 * 6 * 50];
HAL_CANStreamMessage messages[50];
uint32_t messagesRead = 0;
HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[0], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PdpStatus1 pdpStatus;
std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
pdpStatus.bits.chan1_l2) *
0.125;
retData[*count].channel = 1;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
pdpStatus.bits.chan2_l4) *
0.125;
retData[*count].channel = 2;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
pdpStatus.bits.chan3_l6) *
0.125;
retData[*count].channel = 3;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
pdpStatus.bits.chan4_l8) *
0.125;
retData[*count].channel = 4;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
pdpStatus.bits.chan5_l2) *
0.125;
retData[*count].channel = 5;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
pdpStatus.bits.chan6_l4) *
0.125;
retData[*count].channel = 6;
retData[*count].timestamp = timestamp;
(*count)++;
}
messagesRead = 0;
HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[1], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PdpStatus2 pdpStatus;
std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
pdpStatus.bits.chan7_l2) *
0.125;
retData[*count].channel = 7;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
pdpStatus.bits.chan8_l4) *
0.125;
retData[*count].channel = 8;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
pdpStatus.bits.chan9_l6) *
0.125;
retData[*count].channel = 9;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
pdpStatus.bits.chan10_l8) *
0.125;
retData[*count].channel = 10;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
pdpStatus.bits.chan11_l2) *
0.125;
retData[*count].channel = 11;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
pdpStatus.bits.chan12_l4) *
0.125;
retData[*count].channel = 12;
retData[*count].timestamp = timestamp;
(*count)++;
}
messagesRead = 0;
HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[2], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PdpStatus3 pdpStatus;
std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
pdpStatus.bits.chan13_l2) *
0.125;
retData[*count].channel = 13;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
pdpStatus.bits.chan14_l4) *
0.125;
retData[*count].channel = 14;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
pdpStatus.bits.chan15_l6) *
0.125;
retData[*count].channel = 15;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
pdpStatus.bits.chan16_l8) *
0.125;
retData[*count].channel = 16;
retData[*count].timestamp = timestamp;
(*count)++;
}
Exit:
if (*status < 0) {
delete[] retData;
retData = nullptr;
}
return retData;
}
void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status) {
auto pdp = pdpHandles->Get(handle);
if (pdp == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (!pdp->streamHandleAllocated) {
*status = RESOURCE_OUT_OF_RANGE;
return;
}
HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]);
HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[2]);
pdp->streamHandleAllocated = false;
}
} // extern "C"

146
hal/src/main/native/athena/CTREPDP.h
View File

@@ -1,146 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
#include "hal/PowerDistribution.h"
#include "hal/Types.h"
/**
* @defgroup hal_pdp PDP Functions
* @ingroup hal_capi
* Functions to control the Power Distribution Panel.
* @{
*/
#ifdef __cplusplus
extern "C" {
#endif
/**
* Initializes a Power Distribution Panel.
*
* @param module the module number to initialize
* @return the created PDP
*/
HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
int32_t* status);
/**
* Cleans a PDP module.
*
* @param handle the module handle
*/
void HAL_CleanPDP(HAL_PDPHandle handle);
/**
* Gets the module number for a pdp.
*/
int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status);
/**
* Checks if a PDP channel is valid.
*
* @param channel the channel to check
* @return true if the channel is valid, otherwise false
*/
HAL_Bool HAL_CheckPDPChannel(int32_t channel);
/**
* Checks if a PDP module is valid.
*
* @param channel the module to check
* @return true if the module is valid, otherwise false
*/
HAL_Bool HAL_CheckPDPModule(int32_t module);
/**
* Gets the temperature of the PDP.
*
* @param handle the module handle
* @return the module temperature (celsius)
*/
double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status);
/**
* Gets the PDP input voltage.
*
* @param handle the module handle
* @return the input voltage (volts)
*/
double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status);
/**
* Gets the current of a specific PDP channel.
*
* @param module the module
* @param channel the channel
* @return the channel current (amps)
*/
double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
int32_t* status);
/**
* Gets the current of all 16 channels on the PDP.
*
* The array must be large enough to hold all channels.
*
* @param handle the module handle
* @param current the currents (output)
*/
void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
int32_t* status);
/**
* Gets the total current of the PDP.
*
* @param handle the module handle
* @return the total current (amps)
*/
double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status);
/**
* Gets the total power of the PDP.
*
* @param handle the module handle
* @return the total power (watts)
*/
double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status);
/**
* Gets the total energy of the PDP.
*
* @param handle the module handle
* @return the total energy (joules)
*/
double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
/**
* Resets the PDP accumulated energy.
*
* @param handle the module handle
*/
void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
/**
* Clears any PDP sticky faults.
*
* @param handle the module handle
*/
void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status);
void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status);
HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
int32_t* count,
int32_t* status);
void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif
/** @} */

21
hal/src/main/native/athena/Constants.cpp
View File

@@ -1,21 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Constants.h"
#include "ConstantsInternal.h"
using namespace hal;
namespace hal::init {
void InitializeConstants() {}
} // namespace hal::init
extern "C" {
int32_t HAL_GetSystemClockTicksPerMicrosecond(void) {
return kSystemClockTicksPerMicrosecond;
}
} // extern "C"

14
hal/src/main/native/athena/ConstantsInternal.h
View File

@@ -1,14 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
namespace hal {
constexpr int32_t kSystemClockTicksPerMicrosecond = 40;
constexpr int32_t kDutyCycleScaleFactor = 4e7 - 1;
} // namespace hal

376
hal/src/main/native/athena/Counter.cpp
View File

@@ -1,376 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Counter.h"
#include <limits>
#include <memory>
#include <fmt/format.h>
#include "ConstantsInternal.h"
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/HAL.h"
#include "hal/handles/LimitedHandleResource.h"
using namespace hal;
namespace {
struct Counter {
std::unique_ptr<tCounter> counter;
uint8_t index;
};
} // namespace
static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
HAL_HandleEnum::Counter>* counterHandles;
namespace hal::init {
void InitializeCounter() {
static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
HAL_HandleEnum::Counter>
ch;
counterHandles = &ch;
}
} // namespace hal::init
extern "C" {
HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
int32_t* status) {
hal::init::CheckInit();
auto handle = counterHandles->Allocate();
if (handle == HAL_kInvalidHandle) { // out of resources
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto counter = counterHandles->Get(handle);
if (counter == nullptr) { // would only occur on thread issues
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
counter->index = static_cast<uint8_t>(getHandleIndex(handle));
*index = counter->index;
counter->counter.reset(tCounter::create(counter->index, status));
counter->counter->writeConfig_Mode(mode, status);
counter->counter->writeTimerConfig_AverageSize(1, status);
return handle;
}
void HAL_FreeCounter(HAL_CounterHandle counterHandle) {
counterHandles->Free(counterHandle);
}
void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeTimerConfig_AverageSize(size, status);
}
void HAL_SetCounterUpSource(HAL_CounterHandle counterHandle,
HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
bool routingAnalogTrigger = false;
uint8_t routingChannel = 0;
uint8_t routingModule = 0;
bool success =
remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
routingModule, routingAnalogTrigger);
if (!success) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_UpSource_Module(routingModule, status);
counter->counter->writeConfig_UpSource_Channel(routingChannel, status);
counter->counter->writeConfig_UpSource_AnalogTrigger(routingAnalogTrigger,
status);
if (counter->counter->readConfig_Mode(status) == HAL_Counter_kTwoPulse ||
counter->counter->readConfig_Mode(status) ==
HAL_Counter_kExternalDirection) {
HAL_SetCounterUpSourceEdge(counterHandle, true, false, status);
}
counter->counter->strobeReset(status);
}
void HAL_SetCounterUpSourceEdge(HAL_CounterHandle counterHandle,
HAL_Bool risingEdge, HAL_Bool fallingEdge,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_UpRisingEdge(risingEdge, status);
counter->counter->writeConfig_UpFallingEdge(fallingEdge, status);
}
void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_UpFallingEdge(false, status);
counter->counter->writeConfig_UpRisingEdge(false, status);
// Index 0 of digital is always 0.
counter->counter->writeConfig_UpSource_Channel(0, status);
counter->counter->writeConfig_UpSource_AnalogTrigger(false, status);
}
void HAL_SetCounterDownSource(HAL_CounterHandle counterHandle,
HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t mode = counter->counter->readConfig_Mode(status);
if (mode != HAL_Counter_kTwoPulse && mode != HAL_Counter_kExternalDirection) {
// TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only
// supports DownSource in TwoPulse and ExternalDirection modes.");
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status,
"Counter only supports DownSource in TwoPulse and "
"ExternalDirection mode.");
return;
}
bool routingAnalogTrigger = false;
uint8_t routingChannel = 0;
uint8_t routingModule = 0;
bool success =
remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
routingModule, routingAnalogTrigger);
if (!success) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_DownSource_Module(routingModule, status);
counter->counter->writeConfig_DownSource_Channel(routingChannel, status);
counter->counter->writeConfig_DownSource_AnalogTrigger(routingAnalogTrigger,
status);
HAL_SetCounterDownSourceEdge(counterHandle, true, false, status);
counter->counter->strobeReset(status);
}
void HAL_SetCounterDownSourceEdge(HAL_CounterHandle counterHandle,
HAL_Bool risingEdge, HAL_Bool fallingEdge,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_DownRisingEdge(risingEdge, status);
counter->counter->writeConfig_DownFallingEdge(fallingEdge, status);
}
void HAL_ClearCounterDownSource(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_DownFallingEdge(false, status);
counter->counter->writeConfig_DownRisingEdge(false, status);
// Index 0 of digital is always 0.
counter->counter->writeConfig_DownSource_Channel(0, status);
counter->counter->writeConfig_DownSource_AnalogTrigger(false, status);
}
void HAL_SetCounterUpDownMode(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_Mode(HAL_Counter_kTwoPulse, status);
}
void HAL_SetCounterExternalDirectionMode(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_Mode(HAL_Counter_kExternalDirection, status);
}
void HAL_SetCounterSemiPeriodMode(HAL_CounterHandle counterHandle,
HAL_Bool highSemiPeriod, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_Mode(HAL_Counter_kSemiperiod, status);
counter->counter->writeConfig_UpRisingEdge(highSemiPeriod, status);
HAL_SetCounterUpdateWhenEmpty(counterHandle, false, status);
}
void HAL_SetCounterPulseLengthMode(HAL_CounterHandle counterHandle,
double threshold, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeConfig_Mode(HAL_Counter_kPulseLength, status);
counter->counter->writeConfig_PulseLengthThreshold(
static_cast<uint32_t>(threshold * 1.0e6) *
kSystemClockTicksPerMicrosecond,
status);
}
int32_t HAL_GetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return counter->counter->readTimerConfig_AverageSize(status);
}
void HAL_SetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
int32_t samplesToAverage, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (samplesToAverage < 1 || samplesToAverage > 127) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Samples to average must be between "
"1 and 127 inclusive. Requested {}",
samplesToAverage));
return;
}
counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status);
}
void HAL_ResetCounter(HAL_CounterHandle counterHandle, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->strobeReset(status);
}
int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
int32_t value = counter->counter->readOutput_Value(status);
return value;
}
double HAL_GetCounterPeriod(HAL_CounterHandle counterHandle, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0.0;
}
tCounter::tTimerOutput output = counter->counter->readTimerOutput(status);
if (output.Stalled) {
return std::numeric_limits<double>::infinity();
}
// output.Period is a fixed point number that counts by 2 (24 bits, 25
// integer bits)
double period = static_cast<double>(output.Period << 1) /
static_cast<double>(output.Count);
return period * 2.5e-8; // result * timebase (currently 25ns)
}
void HAL_SetCounterMaxPeriod(HAL_CounterHandle counterHandle, double maxPeriod,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeTimerConfig_StallPeriod(
static_cast<uint32_t>(maxPeriod * 4.0e8), status);
}
void HAL_SetCounterUpdateWhenEmpty(HAL_CounterHandle counterHandle,
HAL_Bool enabled, int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
counter->counter->writeTimerConfig_UpdateWhenEmpty(enabled, status);
}
HAL_Bool HAL_GetCounterStopped(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
return counter->counter->readTimerOutput_Stalled(status);
}
HAL_Bool HAL_GetCounterDirection(HAL_CounterHandle counterHandle,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
bool value = counter->counter->readOutput_Direction(status);
return value;
}
void HAL_SetCounterReverseDirection(HAL_CounterHandle counterHandle,
HAL_Bool reverseDirection,
int32_t* status) {
auto counter = counterHandles->Get(counterHandle);
if (counter == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (counter->counter->readConfig_Mode(status) ==
HAL_Counter_kExternalDirection) {
if (reverseDirection) {
HAL_SetCounterDownSourceEdge(counterHandle, true, true, status);
} else {
HAL_SetCounterDownSourceEdge(counterHandle, false, true, status);
}
}
}
} // extern "C"

592
hal/src/main/native/athena/DIO.cpp
View File

@@ -1,592 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/DIO.h"
#include <cmath>
#include <cstdio>
#include <thread>
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/cpp/fpga_clock.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
using namespace hal;
// Create a mutex to protect changes to the DO PWM config
static wpi::mutex digitalPwmMutex;
static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>*
digitalPWMHandles;
namespace hal::init {
void InitializeDIO() {
static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
kNumDigitalPWMOutputs,
HAL_HandleEnum::DigitalPWM>
dpH;
digitalPWMHandles = &dpH;
}
} // namespace hal::init
extern "C" {
HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
HAL_Bool input,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
initializeDigital(status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
int16_t channel = getPortHandleChannel(portHandle);
if (channel == InvalidHandleIndex || channel >= kNumDigitalChannels) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
kNumDigitalChannels, channel);
return HAL_kInvalidHandle;
}
HAL_DigitalHandle handle;
auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO,
&handle, status);
if (*status != 0) {
if (port) {
hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
port->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
kNumDigitalChannels, channel);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
port->channel = static_cast<uint8_t>(channel);
std::scoped_lock lock(digitalDIOMutex);
tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
if (!getPortHandleSPIEnable(portHandle)) {
// if this flag is not set, we actually want DIO.
uint32_t bitToSet = 1u << remapSPIChannel(port->channel);
uint16_t specialFunctions = spiSystem->readEnableDIO(status);
// Set the field to enable SPI DIO
spiSystem->writeEnableDIO(specialFunctions | bitToSet, status);
if (input) {
outputEnable.SPIPort =
outputEnable.SPIPort & (~bitToSet); // clear the field for read
} else {
outputEnable.SPIPort =
outputEnable.SPIPort | bitToSet; // set the bits for write
}
}
} else if (port->channel < kNumDigitalHeaders) {
uint32_t bitToSet = 1u << port->channel;
if (input) {
outputEnable.Headers =
outputEnable.Headers & (~bitToSet); // clear the bit for read
} else {
outputEnable.Headers =
outputEnable.Headers | bitToSet; // set the bit for write
}
} else {
uint32_t bitToSet = 1u << remapMXPChannel(port->channel);
uint16_t specialFunctions =
digitalSystem->readEnableMXPSpecialFunction(status);
digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet,
status);
if (input) {
outputEnable.MXP =
outputEnable.MXP & (~bitToSet); // clear the bit for read
} else {
outputEnable.MXP = outputEnable.MXP | bitToSet; // set the bit for write
}
}
digitalSystem->writeOutputEnable(outputEnable, status);
port->previousAllocation = allocationLocation ? allocationLocation : "";
return handle;
}
HAL_Bool HAL_CheckDIOChannel(int32_t channel) {
return channel < kNumDigitalChannels && channel >= 0;
}
void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
// no status, so no need to check for a proper free.
if (port == nullptr) {
return;
}
digitalChannelHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
// Wait for no other object to hold this handle.
auto start = hal::fpga_clock::now();
while (port.use_count() != 1) {
auto current = hal::fpga_clock::now();
if (start + std::chrono::seconds(1) < current) {
std::puts("DIO handle free timeout");
std::fflush(stdout);
break;
}
std::this_thread::yield();
}
int32_t status = 0;
std::scoped_lock lock(digitalDIOMutex);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
// Unset the SPI flag
int32_t bitToUnset = 1 << remapSPIChannel(port->channel);
uint16_t specialFunctions = spiSystem->readEnableDIO(&status);
spiSystem->writeEnableDIO(specialFunctions & ~bitToUnset, &status);
} else if (port->channel >= kNumDigitalHeaders) {
// Unset the MXP flag
uint32_t bitToUnset = 1u << remapMXPChannel(port->channel);
uint16_t specialFunctions =
digitalSystem->readEnableMXPSpecialFunction(&status);
digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToUnset,
&status);
}
}
void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
}
HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status) {
auto handle = digitalPWMHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto id = digitalPWMHandles->Get(handle);
if (id == nullptr) { // would only occur on thread issue.
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
*id = static_cast<uint8_t>(getHandleIndex(handle));
return handle;
}
void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator) {
digitalPWMHandles->Free(pwmGenerator);
}
void HAL_SetDigitalPWMRate(double rate, int32_t* status) {
// Currently rounding in the log rate domain... heavy weight toward picking a
// higher freq.
// TODO: Round in the linear rate domain.
initializeDigital(status);
if (*status != 0) {
return;
}
uint16_t pwmPeriodPower = std::lround(std::log2(1.0 / (16 * 1.0E-6 * rate)));
digitalSystem->writePWMPeriodPower(pwmPeriodPower, status);
}
void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
double dutyCycle, int32_t* status) {
auto port = digitalPWMHandles->Get(pwmGenerator);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t id = *port;
if (dutyCycle > 1.0) {
dutyCycle = 1.0;
}
if (dutyCycle < 0.0) {
dutyCycle = 0.0;
}
double rawDutyCycle = 256.0 * dutyCycle;
if (rawDutyCycle > 255.5) {
rawDutyCycle = 255.5;
}
{
std::scoped_lock lock(digitalPwmMutex);
uint16_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status);
if (pwmPeriodPower < 4) {
// The resolution of the duty cycle drops close to the highest
// frequencies.
rawDutyCycle = rawDutyCycle / std::pow(2.0, 4 - pwmPeriodPower);
}
if (id < 4) {
digitalSystem->writePWMDutyCycleA(id, static_cast<uint8_t>(rawDutyCycle),
status);
} else {
digitalSystem->writePWMDutyCycleB(
id - 4, static_cast<uint8_t>(rawDutyCycle), status);
}
}
}
void HAL_SetDigitalPWMPPS(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;
digitalSystem->writePWMPeriodPower(0xffff, status);
double rawDutyCycle = 31.0 * dutyCycle;
if (rawDutyCycle > 30.5) {
rawDutyCycle = 30.5;
}
{
std::scoped_lock lock(digitalPwmMutex);
if (id < 4) {
digitalSystem->writePWMDutyCycleA(id, static_cast<uint8_t>(rawDutyCycle),
status);
} else {
digitalSystem->writePWMDutyCycleB(
id - 4, static_cast<uint8_t>(rawDutyCycle), status);
}
}
}
void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
int32_t channel, int32_t* status) {
auto port = digitalPWMHandles->Get(pwmGenerator);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t id = *port;
if (channel >= kNumDigitalHeaders &&
channel <
kNumDigitalHeaders + kNumDigitalMXPChannels) { // If it is on the MXP
/* Then to write as a digital PWM channel an offset is needed to write on
* the correct channel
*/
channel += kMXPDigitalPWMOffset;
}
digitalSystem->writePWMOutputSelect(id, channel, status);
}
void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (value != 0 && value != 1) {
if (value != 0) {
value = 1;
}
}
{
std::scoped_lock lock(digitalDIOMutex);
tDIO::tOutputEnable currentOutputEnable =
digitalSystem->readOutputEnable(status);
HAL_Bool isInput = false;
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
isInput =
((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
1) == 0;
} else if (port->channel < kNumDigitalHeaders) {
isInput = ((currentOutputEnable.Headers >> port->channel) & 1) == 0;
} else {
isInput = ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) &
1) == 0;
}
if (isInput) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, "Cannot set output of an input channel");
return;
}
tDIO::tDO currentDIO = digitalSystem->readDO(status);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
if (value == 0) {
currentDIO.SPIPort =
currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
} else if (value == 1) {
currentDIO.SPIPort =
currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
}
} else if (port->channel < kNumDigitalHeaders) {
if (value == 0) {
currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
} else if (value == 1) {
currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
}
} else {
if (value == 0) {
currentDIO.MXP =
currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
} else if (value == 1) {
currentDIO.MXP =
currentDIO.MXP | (1u << remapMXPChannel(port->channel));
}
}
digitalSystem->writeDO(currentDIO, status);
}
}
void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
{
std::scoped_lock lock(digitalDIOMutex);
tDIO::tOutputEnable currentDIO = digitalSystem->readOutputEnable(status);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
if (input) {
currentDIO.SPIPort =
currentDIO.SPIPort & ~(1u << remapSPIChannel(port->channel));
} else {
currentDIO.SPIPort =
currentDIO.SPIPort | (1u << remapSPIChannel(port->channel));
}
} else if (port->channel < kNumDigitalHeaders) {
if (input) {
currentDIO.Headers = currentDIO.Headers & ~(1u << port->channel);
} else {
currentDIO.Headers = currentDIO.Headers | (1u << port->channel);
}
} else {
if (input) {
currentDIO.MXP =
currentDIO.MXP & ~(1u << remapMXPChannel(port->channel));
} else {
currentDIO.MXP =
currentDIO.MXP | (1u << remapMXPChannel(port->channel));
}
}
digitalSystem->writeOutputEnable(currentDIO, status);
}
}
HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
tDIO::tDI currentDIO = digitalSystem->readDI(status);
// Shift 00000001 over channel-1 places.
// AND it against the currentDIO
// if it == 0, then return false
// else return true
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
return ((currentDIO.SPIPort >> remapSPIChannel(port->channel)) & 1) != 0;
} else if (port->channel < kNumDigitalHeaders) {
return ((currentDIO.Headers >> port->channel) & 1) != 0;
} else {
return ((currentDIO.MXP >> remapMXPChannel(port->channel)) & 1) != 0;
}
}
HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
tDIO::tOutputEnable currentOutputEnable =
digitalSystem->readOutputEnable(status);
// Shift 00000001 over port->channel-1 places.
// AND it against the currentOutputEnable
// if it == 0, then return false
// else return true
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
return ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
1) == 0;
} else if (port->channel < kNumDigitalHeaders) {
return ((currentOutputEnable.Headers >> port->channel) & 1) == 0;
} else {
return ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) & 1) ==
0;
}
}
void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds,
int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint32_t pulseLengthMicroseconds =
static_cast<uint32_t>(pulseLengthSeconds * 1e6);
if (pulseLengthMicroseconds <= 0 || pulseLengthMicroseconds > 0xFFFF) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status,
"Length must be between 1 and 65535 microseconds");
return;
}
tDIO::tPulse pulse;
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
pulse.SPIPort = 1u << remapSPIChannel(port->channel);
} else if (port->channel < kNumDigitalHeaders) {
pulse.Headers = 1u << port->channel;
} else {
pulse.MXP = 1u << remapMXPChannel(port->channel);
}
digitalSystem->writePulseLength(
static_cast<uint16_t>(pulseLengthMicroseconds), status);
digitalSystem->writePulse(pulse, status);
}
void HAL_PulseMultiple(uint32_t channelMask, double pulseLengthSeconds,
int32_t* status) {
uint32_t pulseLengthMicroseconds =
static_cast<uint32_t>(pulseLengthSeconds * 1e6);
if (pulseLengthMicroseconds <= 0 || pulseLengthMicroseconds > 0xFFFF) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status,
"Length must be between 1 and 65535 microseconds");
return;
}
tDIO::tPulse pulse;
pulse.Headers = channelMask & 0x2FF;
pulse.MXP = (channelMask & 0xFFFF) >> 10;
pulse.SPIPort = (channelMask & 0x1F) >> 26;
digitalSystem->writePulseLength(
static_cast<uint16_t>(pulseLengthMicroseconds), status);
digitalSystem->writePulse(pulse, status);
}
HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
return (pulseRegister.SPIPort & (1 << remapSPIChannel(port->channel))) != 0;
} else if (port->channel < kNumDigitalHeaders) {
return (pulseRegister.Headers & (1 << port->channel)) != 0;
} else {
return (pulseRegister.MXP & (1 << remapMXPChannel(port->channel))) != 0;
}
}
HAL_Bool HAL_IsAnyPulsing(int32_t* status) {
initializeDigital(status);
if (*status != 0) {
return false;
}
tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);
return pulseRegister.Headers != 0 && pulseRegister.MXP != 0 &&
pulseRegister.SPIPort != 0;
}
void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex,
int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
std::scoped_lock lock(digitalDIOMutex);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
// Channels 10-15 are SPI channels, so subtract our MXP channels
digitalSystem->writeFilterSelectHdr(port->channel - kNumDigitalMXPChannels,
filterIndex, status);
} else if (port->channel < kNumDigitalHeaders) {
digitalSystem->writeFilterSelectHdr(port->channel, filterIndex, status);
} else {
digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->channel),
filterIndex, status);
}
}
int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
std::scoped_lock lock(digitalDIOMutex);
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
// Channels 10-15 are SPI channels, so subtract our MXP channels
return digitalSystem->readFilterSelectHdr(
port->channel - kNumDigitalMXPChannels, status);
} else if (port->channel < kNumDigitalHeaders) {
return digitalSystem->readFilterSelectHdr(port->channel, status);
} else {
return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->channel),
status);
}
}
void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) {
initializeDigital(status);
if (*status != 0) {
return;
}
std::scoped_lock lock(digitalDIOMutex);
digitalSystem->writeFilterPeriodHdr(filterIndex, value, status);
if (*status == 0) {
digitalSystem->writeFilterPeriodMXP(filterIndex, value, status);
}
}
int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status) {
initializeDigital(status);
if (*status != 0) {
return 0;
}
uint32_t hdrPeriod = 0;
uint32_t mxpPeriod = 0;
{
std::scoped_lock lock(digitalDIOMutex);
hdrPeriod = digitalSystem->readFilterPeriodHdr(filterIndex, status);
if (*status == 0) {
mxpPeriod = digitalSystem->readFilterPeriodMXP(filterIndex, status);
}
}
if (hdrPeriod != mxpPeriod) {
*status = NiFpga_Status_SoftwareFault;
return -1;
}
return hdrPeriod;
}
} // extern "C"

1113
hal/src/main/native/athena/DMA.cpp
View File

File diff suppressed because it is too large Load Diff

202
hal/src/main/native/athena/DigitalInternal.cpp
View File

@@ -1,202 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "DigitalInternal.h"
#include <atomic>
#include <cmath>
#include <memory>
#include <thread>
#include <FRC_NetworkCommunication/LoadOut.h>
#include <wpi/mutex.h>
#include "ConstantsInternal.h"
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/AnalogTrigger.h"
#include "hal/ChipObject.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
#include "hal/cpp/UnsafeDIO.h"
namespace hal {
std::unique_ptr<tDIO> digitalSystem;
std::unique_ptr<tRelay> relaySystem;
std::unique_ptr<tPWM> pwmSystem;
std::unique_ptr<tSPI> spiSystem;
// Create a mutex to protect changes to the digital output values
wpi::mutex digitalDIOMutex;
DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
kNumDigitalChannels + kNumPWMHeaders>*
digitalChannelHandles;
namespace init {
void InitializeDigitalInternal() {
static DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
kNumDigitalChannels + kNumPWMHeaders>
dcH;
digitalChannelHandles = &dcH;
}
} // namespace init
namespace detail {
wpi::mutex& UnsafeGetDIOMutex() {
return digitalDIOMutex;
}
tDIO* UnsafeGetDigitalSystem() {
return digitalSystem.get();
}
int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status) {
auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
tDIO::tDO output;
output.value = 0;
if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
output.SPIPort = (1u << remapSPIChannel(port->channel));
} else if (port->channel < kNumDigitalHeaders) {
output.Headers = (1u << port->channel);
} else {
output.MXP = (1u << remapMXPChannel(port->channel));
}
return output.value;
}
} // namespace detail
void initializeDigital(int32_t* status) {
hal::init::CheckInit();
static std::atomic_bool initialized{false};
static wpi::mutex initializeMutex;
// Initial check, as if it's true initialization has finished
if (initialized) {
return;
}
std::scoped_lock lock(initializeMutex);
// Second check in case another thread was waiting
if (initialized) {
return;
}
digitalSystem.reset(tDIO::create(status));
// Relay Setup
relaySystem.reset(tRelay::create(status));
// Turn off all relay outputs.
relaySystem->writeValue_Forward(0, status);
relaySystem->writeValue_Reverse(0, status);
// PWM Setup
pwmSystem.reset(tPWM::create(status));
// Make sure that the 9403 IONode has had a chance to initialize before
// continuing.
while (pwmSystem->readLoopTiming(status) == 0) {
std::this_thread::yield();
}
if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) {
*status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error
}
// Calculate the length, in ms, of one DIO loop
double loopTime = pwmSystem->readLoopTiming(status) /
(kSystemClockTicksPerMicrosecond * 1e3);
pwmSystem->writeConfig_Period(std::lround(kDefaultPwmPeriod / loopTime),
status);
pwmSystem->writeConfig_MinHigh(0, status);
// Ensure that PWM output values are set to OFF
for (uint8_t pwmIndex = 0; pwmIndex < kNumPWMChannels; pwmIndex++) {
// Copy of SetPWM
if (pwmIndex < tPWM::kNumHdrRegisters) {
pwmSystem->writeHdr(pwmIndex, kPwmDisabled, status);
} else {
pwmSystem->writeMXP(pwmIndex - tPWM::kNumHdrRegisters, kPwmDisabled,
status);
}
// Copy of SetPWMPeriodScale, set to 4x by default.
if (pwmIndex < tPWM::kNumPeriodScaleHdrElements) {
pwmSystem->writePeriodScaleHdr(pwmIndex, 3, status);
} else {
pwmSystem->writePeriodScaleMXP(
pwmIndex - tPWM::kNumPeriodScaleHdrElements, 3, status);
}
}
// SPI setup
spiSystem.reset(tSPI::create(status));
initialized = true;
}
bool remapDigitalSource(HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
uint8_t& channel, uint8_t& module,
bool& analogTrigger) {
if (isHandleType(digitalSourceHandle, HAL_HandleEnum::AnalogTrigger)) {
// If handle passed, index is not negative
int32_t index = getHandleIndex(digitalSourceHandle);
channel = (index << 2) + analogTriggerType;
module = channel >> 4;
analogTrigger = true;
return true;
} else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) {
int32_t index = getHandleIndex(digitalSourceHandle);
if (index >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
// channels 10-15, so need to add headers to remap index
channel = remapSPIChannel(index) + kNumDigitalHeaders;
module = 0;
} else if (index >= kNumDigitalHeaders) {
channel = remapMXPChannel(index);
module = 1;
} else {
channel = index;
module = 0;
}
analogTrigger = false;
return true;
} else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::PWM)) {
// PWM's on MXP port are supported as a digital source
int32_t index = getHandleIndex(digitalSourceHandle);
if (index >= kNumPWMHeaders) {
channel = remapMXPPWMChannel(index);
module = 1;
analogTrigger = false;
return true;
}
}
return false;
}
int32_t remapMXPChannel(int32_t channel) {
return channel - 10;
}
int32_t remapMXPPWMChannel(int32_t channel) {
if (channel < 14) {
return channel - 10; // first block of 4 pwms (MXP 0-3)
} else {
return channel - 6; // block of PWMs after SPI
}
}
int32_t remapSPIChannel(int32_t channel) {
return channel - 26;
}
} // namespace hal
// Unused function here to test template compile.
__attribute__((unused)) static void CompileFunctorTest() {
hal::UnsafeManipulateDIO(0, nullptr, [](hal::DIOSetProxy& proxy) {});
}

121
hal/src/main/native/athena/DigitalInternal.h
View File

@@ -1,121 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
#include <memory>
#include <string>
#include <wpi/mutex.h>
#include "PortsInternal.h"
#include "hal/AnalogTrigger.h"
#include "hal/ChipObject.h"
#include "hal/Ports.h"
#include "hal/Types.h"
#include "hal/handles/DigitalHandleResource.h"
#include "hal/handles/HandlesInternal.h"
namespace hal {
/**
* MXP channels when used as digital output PWM are offset from actual value
*/
constexpr int32_t kMXPDigitalPWMOffset = 6;
constexpr int32_t kExpectedLoopTiming = 40;
/**
* kDefaultPwmPeriod is in ms
*
* - 20ms periods (50 Hz) are the "safest" setting in that this works for all
* devices
* - 20ms periods seem to be desirable for Vex Motors
* - 20ms periods are the specified period for HS-322HD servos, but work
* reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums
* and get hot; by 5.0ms the hum is nearly continuous
* - 10ms periods work well for Victor 884
* - 5ms periods allows higher update rates for Luminary Micro Jaguar motor
* 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;
constexpr int32_t kPwmDisabled = 0;
constexpr int32_t kPwmAlwaysHigh = 0xFFFF;
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;
std::string previousAllocation;
};
extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
kNumDigitalChannels + kNumPWMHeaders>*
digitalChannelHandles;
extern wpi::mutex digitalDIOMutex;
/**
* Initialize the digital system.
*/
void initializeDigital(int32_t* status);
/**
* remap the digital source channel and set the module.
* If it's an analog trigger, determine the module from the high order routing
* channel else do normal digital input remapping based on channel number
* (MXP)
*/
bool remapDigitalSource(HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
uint8_t& channel, uint8_t& module, bool& analogTrigger);
/**
* Remap the Digital Channel to map to the bitfield channel of the FPGA
*/
constexpr int32_t remapDigitalChannelToBitfieldChannel(int32_t channel) {
// First 10 are headers
if (channel < kNumDigitalHeaders) {
return channel;
// 2nd group of 16 are mxp. So if mxp port, add 6, since they start at 10
} else if (channel < kNumDigitalMXPChannels) {
return channel + 6;
// Assume SPI, so remove MXP channels
} else {
return channel - kNumDigitalMXPChannels;
}
}
/**
* Map DIO channel numbers from their physical number (10 to 26) to their
* position in the bit field.
*/
int32_t remapMXPChannel(int32_t channel);
int32_t remapMXPPWMChannel(int32_t channel);
/**
* Map SPI channel numbers from their physical number (27 to 31) to their
* position in the bit field.
*/
int32_t remapSPIChannel(int32_t channel);
} // namespace hal

136
hal/src/main/native/athena/DutyCycle.cpp
View File

@@ -1,136 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/DutyCycle.h"
#include <memory>
#include "ConstantsInternal.h"
#include "DigitalInternal.h"
#include "DutyCycleInternal.h"
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/ChipObject.h"
#include "hal/Errors.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
using namespace hal;
namespace hal {
LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
namespace init {
void InitializeDutyCycle() {
static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
HAL_HandleEnum::DutyCycle>
dcH;
dutyCycleHandles = &dcH;
}
} // namespace init
} // namespace hal
extern "C" {
HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType triggerType,
int32_t* status) {
hal::init::CheckInit();
bool routingAnalogTrigger = false;
uint8_t routingChannel = 0;
uint8_t routingModule = 0;
bool success =
remapDigitalSource(digitalSourceHandle, triggerType, routingChannel,
routingModule, routingAnalogTrigger);
if (!success) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
HAL_DutyCycleHandle handle = dutyCycleHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto dutyCycle = dutyCycleHandles->Get(handle);
uint32_t index = static_cast<uint32_t>(getHandleIndex(handle));
dutyCycle->dutyCycle.reset(tDutyCycle::create(index, status));
dutyCycle->dutyCycle->writeSource_AnalogTrigger(routingAnalogTrigger, status);
dutyCycle->dutyCycle->writeSource_Channel(routingChannel, status);
dutyCycle->dutyCycle->writeSource_Module(routingModule, status);
dutyCycle->index = index;
return handle;
}
void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
// Just free it, the unique ptr will take care of everything else
dutyCycleHandles->Free(dutyCycleHandle);
}
void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
HAL_SimDeviceHandle device) {}
int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
int32_t* status) {
auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
if (!dutyCycle) {
*status = HAL_HANDLE_ERROR;
return 0;
}
// TODO Handle Overflow
unsigned char overflow = 0;
return dutyCycle->dutyCycle->readFrequency(&overflow, status);
}
double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
int32_t* status) {
auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
if (!dutyCycle) {
*status = HAL_HANDLE_ERROR;
return 0;
}
// TODO Handle Overflow
unsigned char overflow = 0;
uint32_t output = dutyCycle->dutyCycle->readOutput(&overflow, status);
return output / static_cast<double>(kDutyCycleScaleFactor);
}
int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
int32_t* status) {
auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
if (!dutyCycle) {
*status = HAL_HANDLE_ERROR;
return 0;
}
// TODO Handle Overflow
unsigned char overflow = 0;
uint32_t highTime = dutyCycle->dutyCycle->readHighTicks(&overflow, status);
if (*status != 0) {
return 0;
}
// Output will be at max 4e7, so x25 will still fit in a 32 bit signed int.
return highTime * 25;
}
int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
int32_t* status) {
return kDutyCycleScaleFactor;
}
int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
int32_t* status) {
auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
if (!dutyCycle) {
*status = HAL_HANDLE_ERROR;
return -1;
}
return dutyCycle->index;
}
} // extern "C"

21
hal/src/main/native/athena/DutyCycleInternal.h
View File

@@ -1,21 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include "hal/ChipObject.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
namespace hal {
struct DutyCycle {
std::unique_ptr<tDutyCycle> dutyCycle;
int index;
};
extern LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
} // namespace hal

493
hal/src/main/native/athena/Encoder.cpp
View File

@@ -1,493 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Encoder.h"
#include <memory>
#include <fmt/format.h>
#include "EncoderInternal.h"
#include "FPGAEncoder.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/ChipObject.h"
#include "hal/Counter.h"
#include "hal/Errors.h"
#include "hal/handles/LimitedClassedHandleResource.h"
using namespace hal;
Encoder::Encoder(HAL_Handle digitalSourceHandleA,
HAL_AnalogTriggerType analogTriggerTypeA,
HAL_Handle digitalSourceHandleB,
HAL_AnalogTriggerType analogTriggerTypeB,
bool reverseDirection, HAL_EncoderEncodingType encodingType,
int32_t* status) {
m_encodingType = encodingType;
switch (encodingType) {
case HAL_Encoder_k4X: {
m_encodingScale = 4;
m_encoder = HAL_InitializeFPGAEncoder(
digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
analogTriggerTypeB, reverseDirection, &m_index, status);
if (*status != 0) {
return;
}
m_counter = HAL_kInvalidHandle;
SetMaxPeriod(0.5, status);
break;
}
case HAL_Encoder_k1X:
case HAL_Encoder_k2X: {
SetupCounter(digitalSourceHandleA, analogTriggerTypeA,
digitalSourceHandleB, analogTriggerTypeB, reverseDirection,
encodingType, status);
m_encodingScale = encodingType == HAL_Encoder_k1X ? 1 : 2;
break;
}
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Encoding type {} invalid.",
static_cast<int>(encodingType)));
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) {
HAL_FreeCounter(m_counter);
} else {
HAL_FreeFPGAEncoder(m_encoder);
}
}
// 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;
hal::SetLastError(status, fmt::format("Samples to average must be between "
"1 and 127 inclusive. Requested {}",
samplesToAverage));
return;
}
if (m_counter) {
HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, status);
} else {
HAL_SetFPGAEncoderSamplesToAverage(m_encoder, samplesToAverage, status);
}
}
int32_t Encoder::GetSamplesToAverage(int32_t* status) const {
if (m_counter) {
return HAL_GetCounterSamplesToAverage(m_counter, status);
} else {
return HAL_GetFPGAEncoderSamplesToAverage(m_encoder, status);
}
}
void Encoder::SetIndexSource(HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_EncoderIndexingType type, int32_t* status) {
if (m_counter) {
*status = HAL_COUNTER_NOT_SUPPORTED;
return;
}
bool activeHigh =
(type == HAL_kResetWhileHigh) || (type == HAL_kResetOnRisingEdge);
bool edgeSensitive =
(type == HAL_kResetOnFallingEdge) || (type == HAL_kResetOnRisingEdge);
HAL_SetFPGAEncoderIndexSource(m_encoder, digitalSourceHandle,
analogTriggerType, activeHigh, edgeSensitive,
status);
}
double Encoder::DecodingScaleFactor() const {
switch (m_encodingType) {
case HAL_Encoder_k1X:
return 1.0;
case HAL_Encoder_k2X:
return 0.5;
case HAL_Encoder_k4X:
return 0.25;
default:
return 0.0;
}
}
static LimitedClassedHandleResource<HAL_EncoderHandle, Encoder,
kNumEncoders + kNumCounters,
HAL_HandleEnum::Encoder>* encoderHandles;
namespace hal::init {
void InitializeEncoder() {
static LimitedClassedHandleResource<HAL_EncoderHandle, Encoder,
kNumEncoders + kNumCounters,
HAL_HandleEnum::Encoder>
eH;
encoderHandles = &eH;
}
} // namespace hal::init
namespace hal {
bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
HAL_FPGAEncoderHandle* fpgaHandle,
HAL_CounterHandle* counterHandle) {
auto encoder = encoderHandles->Get(handle);
if (!encoder) {
return false;
}
*fpgaHandle = encoder->m_encoder;
*counterHandle = encoder->m_counter;
return true;
}
} // namespace hal
extern "C" {
HAL_EncoderHandle HAL_InitializeEncoder(
HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
HAL_Bool reverseDirection, HAL_EncoderEncodingType encodingType,
int32_t* status) {
hal::init::CheckInit();
auto encoder = std::make_shared<Encoder>(
digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
analogTriggerTypeB, reverseDirection, encodingType, status);
if (*status != 0) {
return HAL_kInvalidHandle; // return in creation error
}
auto handle = encoderHandles->Allocate(encoder);
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
return handle;
}
void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle) {
encoderHandles->Free(encoderHandle);
}
void HAL_SetEncoderSimDevice(HAL_EncoderHandle handle,
HAL_SimDeviceHandle device) {}
int32_t HAL_GetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->Get(status);
}
int32_t HAL_GetEncoderRaw(HAL_EncoderHandle encoderHandle, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetRaw(status);
}
int32_t HAL_GetEncoderEncodingScale(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetEncodingScale(status);
}
void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->Reset(status);
}
double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetPeriod(status);
}
void HAL_SetEncoderMaxPeriod(HAL_EncoderHandle encoderHandle, double maxPeriod,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->SetMaxPeriod(maxPeriod, status);
}
HAL_Bool HAL_GetEncoderStopped(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetStopped(status);
}
HAL_Bool HAL_GetEncoderDirection(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetDirection(status);
}
double HAL_GetEncoderDistance(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetDistance(status);
}
double HAL_GetEncoderRate(HAL_EncoderHandle encoderHandle, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetRate(status);
}
void HAL_SetEncoderMinRate(HAL_EncoderHandle encoderHandle, double minRate,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->SetMinRate(minRate, status);
}
void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
double distancePerPulse, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->SetDistancePerPulse(distancePerPulse, status);
}
void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle,
HAL_Bool reverseDirection,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->SetReverseDirection(reverseDirection, status);
}
void HAL_SetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
int32_t samplesToAverage, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->SetSamplesToAverage(samplesToAverage, status);
}
int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetSamplesToAverage(status);
}
double HAL_GetEncoderDecodingScaleFactor(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->DecodingScaleFactor();
}
double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetDistancePerPulse();
}
HAL_EncoderEncodingType HAL_GetEncoderEncodingType(
HAL_EncoderHandle encoderHandle, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return HAL_Encoder_k4X; // default to k4X
}
return encoder->GetEncodingType();
}
void HAL_SetEncoderIndexSource(HAL_EncoderHandle encoderHandle,
HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_EncoderIndexingType type, int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->SetIndexSource(digitalSourceHandle, analogTriggerType, type, status);
}
int32_t HAL_GetEncoderFPGAIndex(HAL_EncoderHandle encoderHandle,
int32_t* status) {
auto encoder = encoderHandles->Get(encoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->GetFPGAIndex();
}
} // extern "C"

83
hal/src/main/native/athena/EncoderInternal.h
View File

@@ -1,83 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
#include "hal/Encoder.h"
namespace hal {
bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
HAL_FPGAEncoderHandle* fpgaEncoderHandle,
HAL_CounterHandle* counterHandle);
class Encoder {
public:
friend bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
HAL_FPGAEncoderHandle* fpgaEncoderHandle,
HAL_CounterHandle* counterHandle);
Encoder(HAL_Handle digitalSourceHandleA,
HAL_AnalogTriggerType analogTriggerTypeA,
HAL_Handle digitalSourceHandleB,
HAL_AnalogTriggerType analogTriggerTypeB, bool reverseDirection,
HAL_EncoderEncodingType encodingType, int32_t* status);
~Encoder();
// CounterBase interface
int32_t Get(int32_t* status) const;
int32_t GetRaw(int32_t* status) const;
int32_t GetEncodingScale(int32_t* status) const;
void Reset(int32_t* status);
double GetPeriod(int32_t* status) const;
void SetMaxPeriod(double maxPeriod, int32_t* status);
bool GetStopped(int32_t* status) const;
bool GetDirection(int32_t* status) const;
double GetDistance(int32_t* status) const;
double GetRate(int32_t* status) const;
void SetMinRate(double minRate, int32_t* status);
void SetDistancePerPulse(double distancePerPulse, int32_t* status);
void SetReverseDirection(bool reverseDirection, int32_t* status);
void SetSamplesToAverage(int32_t samplesToAverage, int32_t* status);
int32_t GetSamplesToAverage(int32_t* status) const;
void SetIndexSource(HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_EncoderIndexingType type, int32_t* status);
int32_t GetFPGAIndex() const { return m_index; }
int32_t GetEncodingScale() const { return m_encodingScale; }
double DecodingScaleFactor() const;
double GetDistancePerPulse() const { return m_distancePerPulse; }
HAL_EncoderEncodingType GetEncodingType() const { return m_encodingType; }
private:
void SetupCounter(HAL_Handle digitalSourceHandleA,
HAL_AnalogTriggerType analogTriggerTypeA,
HAL_Handle digitalSourceHandleB,
HAL_AnalogTriggerType analogTriggerTypeB,
bool reverseDirection, HAL_EncoderEncodingType encodingType,
int32_t* status);
HAL_FPGAEncoderHandle m_encoder = HAL_kInvalidHandle;
HAL_CounterHandle m_counter = HAL_kInvalidHandle;
int32_t m_index = 0;
double m_distancePerPulse = 1.0;
HAL_EncoderEncodingType m_encodingType;
int32_t m_encodingScale;
};
} // namespace hal

66
hal/src/main/native/athena/FPGACalls.cpp
View File

@@ -1,66 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "FPGACalls.h"
#include <cerrno>
#include "dlfcn.h"
#include "hal/Errors.h"
static void* NiFpgaLibrary = nullptr;
namespace hal {
HAL_NiFpga_ReserveIrqContextFunc HAL_NiFpga_ReserveIrqContext;
HAL_NiFpga_UnreserveIrqContextFunc HAL_NiFpga_UnreserveIrqContext;
HAL_NiFpga_WaitOnIrqsFunc HAL_NiFpga_WaitOnIrqs;
HAL_NiFpga_AcknowledgeIrqsFunc HAL_NiFpga_AcknowledgeIrqs;
HAL_NiFpga_OpenHmbFunc HAL_NiFpga_OpenHmb;
HAL_NiFpga_CloseHmbFunc HAL_NiFpga_CloseHmb;
namespace init {
int InitializeFPGA() {
NiFpgaLibrary = dlopen("libNiFpga.so", RTLD_LAZY);
if (!NiFpgaLibrary) {
return errno;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
HAL_NiFpga_ReserveIrqContext =
reinterpret_cast<HAL_NiFpga_ReserveIrqContextFunc>(
dlsym(NiFpgaLibrary, "NiFpgaDll_ReserveIrqContext"));
HAL_NiFpga_UnreserveIrqContext =
reinterpret_cast<HAL_NiFpga_UnreserveIrqContextFunc>(
dlsym(NiFpgaLibrary, "NiFpgaDll_UnreserveIrqContext"));
HAL_NiFpga_WaitOnIrqs = reinterpret_cast<HAL_NiFpga_WaitOnIrqsFunc>(
dlsym(NiFpgaLibrary, "NiFpgaDll_WaitOnIrqs"));
HAL_NiFpga_AcknowledgeIrqs = reinterpret_cast<HAL_NiFpga_AcknowledgeIrqsFunc>(
dlsym(NiFpgaLibrary, "NiFpgaDll_AcknowledgeIrqs"));
HAL_NiFpga_OpenHmb = reinterpret_cast<HAL_NiFpga_OpenHmbFunc>(
dlsym(NiFpgaLibrary, "NiFpgaDll_OpenHmb"));
HAL_NiFpga_CloseHmb = reinterpret_cast<HAL_NiFpga_CloseHmbFunc>(
dlsym(NiFpgaLibrary, "NiFpgaDll_CloseHmb"));
#pragma GCC diagnostic pop
if (HAL_NiFpga_ReserveIrqContext == nullptr ||
HAL_NiFpga_UnreserveIrqContext == nullptr ||
HAL_NiFpga_WaitOnIrqs == nullptr ||
HAL_NiFpga_AcknowledgeIrqs == nullptr || HAL_NiFpga_OpenHmb == nullptr ||
HAL_NiFpga_CloseHmb == nullptr) {
HAL_NiFpga_ReserveIrqContext = nullptr;
HAL_NiFpga_UnreserveIrqContext = nullptr;
HAL_NiFpga_WaitOnIrqs = nullptr;
HAL_NiFpga_AcknowledgeIrqs = nullptr;
HAL_NiFpga_OpenHmb = nullptr;
HAL_NiFpga_CloseHmb = nullptr;
dlclose(NiFpgaLibrary);
NiFpgaLibrary = nullptr;
return NO_AVAILABLE_RESOURCES;
}
return HAL_SUCCESS;
}
} // namespace init
} // namespace hal

47
hal/src/main/native/athena/FPGACalls.h
View File

@@ -1,47 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga.h>
namespace hal {
namespace init {
[[nodiscard]]
int InitializeFPGA();
} // namespace init
using HAL_NiFpga_ReserveIrqContextFunc =
NiFpga_Status (*)(NiFpga_Session session, NiFpga_IrqContext* context);
extern HAL_NiFpga_ReserveIrqContextFunc HAL_NiFpga_ReserveIrqContext;
using HAL_NiFpga_UnreserveIrqContextFunc =
NiFpga_Status (*)(NiFpga_Session session, NiFpga_IrqContext context);
extern HAL_NiFpga_UnreserveIrqContextFunc HAL_NiFpga_UnreserveIrqContext;
using HAL_NiFpga_WaitOnIrqsFunc = NiFpga_Status (*)(
NiFpga_Session session, NiFpga_IrqContext context, uint32_t irqs,
uint32_t timeout, uint32_t* irqsAsserted, NiFpga_Bool* timedOut);
extern HAL_NiFpga_WaitOnIrqsFunc HAL_NiFpga_WaitOnIrqs;
using HAL_NiFpga_AcknowledgeIrqsFunc = NiFpga_Status (*)(NiFpga_Session session,
uint32_t irqs);
extern HAL_NiFpga_AcknowledgeIrqsFunc HAL_NiFpga_AcknowledgeIrqs;
using HAL_NiFpga_OpenHmbFunc = NiFpga_Status (*)(const NiFpga_Session session,
const char* memoryName,
size_t* memorySize,
void** virtualAddress);
extern HAL_NiFpga_OpenHmbFunc HAL_NiFpga_OpenHmb;
using HAL_NiFpga_CloseHmbFunc = NiFpga_Status (*)(const NiFpga_Session session,
const char* memoryName);
extern HAL_NiFpga_CloseHmbFunc HAL_NiFpga_CloseHmb;
} // namespace hal

243
hal/src/main/native/athena/FPGAEncoder.cpp
View File

@@ -1,243 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "FPGAEncoder.h"
#include <limits>
#include <memory>
#include <fmt/format.h>
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/handles/LimitedHandleResource.h"
using namespace hal;
namespace {
struct Encoder {
std::unique_ptr<tEncoder> encoder;
uint8_t index;
};
} // namespace
static constexpr double DECODING_SCALING_FACTOR = 0.25;
static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
HAL_HandleEnum::FPGAEncoder>* fpgaEncoderHandles;
namespace hal::init {
void InitializeFPGAEncoder() {
static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
HAL_HandleEnum::FPGAEncoder>
feH;
fpgaEncoderHandles = &feH;
}
} // namespace hal::init
extern "C" {
HAL_FPGAEncoderHandle HAL_InitializeFPGAEncoder(
HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
HAL_Handle digitalSourceHandleB, HAL_AnalogTriggerType analogTriggerTypeB,
HAL_Bool reverseDirection, int32_t* index, int32_t* status) {
hal::init::CheckInit();
bool routingAnalogTriggerA = false;
uint8_t routingChannelA = 0;
uint8_t routingModuleA = 0;
bool successA = remapDigitalSource(digitalSourceHandleA, analogTriggerTypeA,
routingChannelA, routingModuleA,
routingAnalogTriggerA);
bool routingAnalogTriggerB = false;
uint8_t routingChannelB = 0;
uint8_t routingModuleB = 0;
bool successB = remapDigitalSource(digitalSourceHandleB, analogTriggerTypeB,
routingChannelB, routingModuleB,
routingAnalogTriggerB);
if (!successA || !successB) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
auto handle = fpgaEncoderHandles->Allocate();
if (handle == HAL_kInvalidHandle) { // out of resources
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto encoder = fpgaEncoderHandles->Get(handle);
if (encoder == nullptr) { // will only error on thread issue
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
encoder->index = static_cast<uint8_t>(getHandleIndex(handle));
*index = encoder->index;
// TODO: if (index == ~0ul) { CloneError(quadEncoders); return; }
encoder->encoder.reset(tEncoder::create(encoder->index, status));
encoder->encoder->writeConfig_ASource_Module(routingModuleA, status);
encoder->encoder->writeConfig_ASource_Channel(routingChannelA, status);
encoder->encoder->writeConfig_ASource_AnalogTrigger(routingAnalogTriggerA,
status);
encoder->encoder->writeConfig_BSource_Module(routingModuleB, status);
encoder->encoder->writeConfig_BSource_Channel(routingChannelB, status);
encoder->encoder->writeConfig_BSource_AnalogTrigger(routingAnalogTriggerB,
status);
encoder->encoder->strobeReset(status);
encoder->encoder->writeConfig_Reverse(reverseDirection, status);
encoder->encoder->writeTimerConfig_AverageSize(4, status);
return handle;
}
void HAL_FreeFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle) {
fpgaEncoderHandles->Free(fpgaEncoderHandle);
}
void HAL_ResetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->encoder->strobeReset(status);
}
int32_t HAL_GetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->encoder->readOutput_Value(status);
}
double HAL_GetFPGAEncoderPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0.0;
}
tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status);
if (output.Stalled) {
return std::numeric_limits<double>::infinity();
}
// output.Period is a fixed point number that counts by 2 (24 bits, 25
// integer bits)
double value = static_cast<double>(output.Period << 1) /
static_cast<double>(output.Count);
double measuredPeriod = value * 2.5e-8; // result * timebase (currently 25ns)
return measuredPeriod / DECODING_SCALING_FACTOR;
}
void HAL_SetFPGAEncoderMaxPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
double maxPeriod, int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->encoder->writeTimerConfig_StallPeriod(
static_cast<uint32_t>(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR),
status);
}
HAL_Bool HAL_GetFPGAEncoderStopped(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
return encoder->encoder->readTimerOutput_Stalled(status) != 0;
}
HAL_Bool HAL_GetFPGAEncoderDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
return encoder->encoder->readOutput_Direction(status);
}
void HAL_SetFPGAEncoderReverseDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
HAL_Bool reverseDirection,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->encoder->writeConfig_Reverse(reverseDirection, status);
}
void HAL_SetFPGAEncoderSamplesToAverage(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t samplesToAverage,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (samplesToAverage < 1 || samplesToAverage > 127) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Samples to average must be between "
"1 and 127 inclusive. Requested {}",
samplesToAverage));
return;
}
encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status);
}
int32_t HAL_GetFPGAEncoderSamplesToAverage(
HAL_FPGAEncoderHandle fpgaEncoderHandle, int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
return encoder->encoder->readTimerConfig_AverageSize(status);
}
void HAL_SetFPGAEncoderIndexSource(HAL_FPGAEncoderHandle fpgaEncoderHandle,
HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_Bool activeHigh, HAL_Bool edgeSensitive,
int32_t* status) {
auto encoder = fpgaEncoderHandles->Get(fpgaEncoderHandle);
if (encoder == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
bool routingAnalogTrigger = false;
uint8_t routingChannel = 0;
uint8_t routingModule = 0;
bool success =
remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
routingModule, routingAnalogTrigger);
if (!success) {
*status = HAL_HANDLE_ERROR;
return;
}
encoder->encoder->writeConfig_IndexSource_Channel(routingChannel, status);
encoder->encoder->writeConfig_IndexSource_Module(routingModule, status);
encoder->encoder->writeConfig_IndexSource_AnalogTrigger(routingAnalogTrigger,
status);
encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status);
encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status);
}
} // extern "C"

122
hal/src/main/native/athena/FPGAEncoder.h
View File

@@ -1,122 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this 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);
/**
* Reset the Encoder distance to zero.
* Resets the current count to zero on the encoder.
*/
void HAL_ResetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status);
/**
* Gets the fpga value from the encoder.
* The fpga value is the actual count unscaled by the 1x, 2x, or 4x scale
* factor.
* @return Current fpga count from the encoder
*/
int32_t HAL_GetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status); // Raw value
/**
* Returns the period of the most recent pulse.
* Returns the period of the most recent Encoder pulse in seconds.
* This method compensates for the decoding type.
*
* @deprecated Use GetRate() in favor of this method. This returns unscaled
* periods and GetRate() scales using value from SetDistancePerPulse().
*
* @return Period in seconds of the most recent pulse.
*/
double HAL_GetFPGAEncoderPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status);
/**
* Sets the maximum period for stopped detection.
* Sets the value that represents the maximum period of the Encoder before it
* will assume that the attached device is stopped. This timeout allows users
* to determine if the wheels or other shaft has stopped rotating.
* This method compensates for the decoding type.
*
* @deprecated Use SetMinRate() in favor of this method. This takes unscaled
* periods and SetMinRate() scales using value from SetDistancePerPulse().
*
* @param maxPeriod The maximum time between rising and falling edges before the
* FPGA will
* report the device stopped. This is expressed in seconds.
*/
void HAL_SetFPGAEncoderMaxPeriod(HAL_FPGAEncoderHandle fpgaEncoderHandle,
double maxPeriod, int32_t* status);
/**
* Determine if the encoder is stopped.
* Using the MaxPeriod value, a boolean is returned that is true if the encoder
* is considered stopped and false if it is still moving. A stopped encoder is
* one where the most recent pulse width exceeds the MaxPeriod.
* @return True if the encoder is considered stopped.
*/
HAL_Bool HAL_GetFPGAEncoderStopped(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status);
/**
* The last direction the encoder value changed.
* @return The last direction the encoder value changed.
*/
HAL_Bool HAL_GetFPGAEncoderDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t* status);
/**
* Set the direction sensing for this encoder.
* This sets the direction sensing on the encoder so that it could count in the
* correct software direction regardless of the mounting.
* @param reverseDirection true if the encoder direction should be reversed
*/
void HAL_SetFPGAEncoderReverseDirection(HAL_FPGAEncoderHandle fpgaEncoderHandle,
HAL_Bool reverseDirection,
int32_t* status);
/**
* Set the Samples to Average which specifies the number of samples of the timer
* to average when calculating the period. Perform averaging to account for
* mechanical imperfections or as oversampling to increase resolution.
* @param samplesToAverage The number of samples to average from 1 to 127.
*/
void HAL_SetFPGAEncoderSamplesToAverage(HAL_FPGAEncoderHandle fpgaEncoderHandle,
int32_t samplesToAverage,
int32_t* status);
/**
* Get the Samples to Average which specifies the number of samples of the timer
* to average when calculating the period. Perform averaging to account for
* mechanical imperfections or as oversampling to increase resolution.
* @return SamplesToAverage The number of samples being averaged (from 1 to 127)
*/
int32_t HAL_GetFPGAEncoderSamplesToAverage(
HAL_FPGAEncoderHandle fpgaEncoderHandle, int32_t* status);
/**
* Set an index source for an encoder, which is an input that resets the
* encoder's count.
*/
void HAL_SetFPGAEncoderIndexSource(HAL_FPGAEncoderHandle fpgaEncoderHandle,
HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_Bool activeHigh, HAL_Bool edgeSensitive,
int32_t* status);
} // extern "C"

603
hal/src/main/native/athena/FRCDriverStation.cpp
View File

@@ -1,603 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <atomic>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <limits>
#include <string>
#include <string_view>
#include <FRC_NetworkCommunication/FRCComm.h>
#include <FRC_NetworkCommunication/NetCommRPCProxy_Occur.h>
#include <fmt/format.h>
#include <wpi/EventVector.h>
#include <wpi/SafeThread.h>
#include <wpi/SmallVector.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include "HALInitializer.h"
#include "hal/DriverStation.h"
#include "hal/Errors.h"
static_assert(sizeof(int32_t) >= sizeof(int),
"FRC_NetworkComm status variable is larger than 32 bits");
namespace {
struct HAL_JoystickAxesInt {
int16_t count;
int16_t axes[HAL_kMaxJoystickAxes];
};
} // namespace
namespace {
struct JoystickDataCache {
JoystickDataCache() { std::memset(this, 0, sizeof(*this)); }
void Update();
HAL_JoystickAxes axes[HAL_kMaxJoysticks];
HAL_JoystickPOVs povs[HAL_kMaxJoysticks];
HAL_JoystickButtons buttons[HAL_kMaxJoysticks];
HAL_AllianceStationID allianceStation;
float matchTime;
HAL_ControlWord controlWord;
};
static_assert(std::is_standard_layout_v<JoystickDataCache>);
// static_assert(std::is_trivial_v<JoystickDataCache>);
struct FRCDriverStation {
wpi::EventVector newDataEvents;
};
} // namespace
static ::FRCDriverStation* driverStation;
// Message and Data variables
static wpi::mutex msgMutex;
static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum,
HAL_JoystickAxes* axes) {
HAL_JoystickAxesInt netcommAxes;
int retVal = FRC_NetworkCommunication_getJoystickAxes(
joystickNum, reinterpret_cast<JoystickAxes_t*>(&netcommAxes),
HAL_kMaxJoystickAxes);
// copy integer values to double values
axes->count = netcommAxes.count;
// current scaling is -128 to 127, can easily be patched in the future by
// changing this function.
for (int32_t i = 0; i < netcommAxes.count; i++) {
int8_t value = netcommAxes.axes[i];
axes->raw[i] = value;
if (value < 0) {
axes->axes[i] = value / 128.0;
} else {
axes->axes[i] = value / 127.0;
}
}
return retVal;
}
static int32_t HAL_GetJoystickPOVsInternal(int32_t joystickNum,
HAL_JoystickPOVs* povs) {
return FRC_NetworkCommunication_getJoystickPOVs(
joystickNum, reinterpret_cast<JoystickPOV_t*>(povs),
HAL_kMaxJoystickPOVs);
}
static int32_t HAL_GetJoystickButtonsInternal(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
return FRC_NetworkCommunication_getJoystickButtons(
joystickNum, &buttons->buttons, &buttons->count);
}
void JoystickDataCache::Update() {
for (int i = 0; i < HAL_kMaxJoysticks; i++) {
HAL_GetJoystickAxesInternal(i, &axes[i]);
HAL_GetJoystickPOVsInternal(i, &povs[i]);
HAL_GetJoystickButtonsInternal(i, &buttons[i]);
}
AllianceStationID_t alliance = kAllianceStationID_red1;
FRC_NetworkCommunication_getAllianceStation(&alliance);
int allianceInt = alliance;
allianceInt += 1;
allianceStation = static_cast<HAL_AllianceStationID>(allianceInt);
FRC_NetworkCommunication_getMatchTime(&matchTime);
FRC_NetworkCommunication_getControlWord(
reinterpret_cast<ControlWord_t*>(&controlWord));
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
if ((stickNum) < 0 || (stickNum) >= HAL_kMaxJoysticks) \
return PARAMETER_OUT_OF_RANGE
static HAL_ControlWord newestControlWord;
static JoystickDataCache caches[3];
static JoystickDataCache* currentRead = &caches[0];
static JoystickDataCache* currentReadLocal = &caches[0];
static std::atomic<JoystickDataCache*> currentCache{nullptr};
static JoystickDataCache* lastGiven = &caches[1];
static JoystickDataCache* cacheToUpdate = &caches[2];
static wpi::mutex cacheMutex;
/**
* Retrieve the Joystick Descriptor for particular slot.
*
* @param[out] desc descriptor (data transfer object) to fill in. desc is filled
* in regardless of success. In other words, if descriptor is
* not available, desc is filled in with default values
* matching the init-values in Java and C++ Driverstation for
* when caller requests a too-large joystick index.
* @return error code reported from Network Comm back-end. Zero is good,
* nonzero is bad.
*/
static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
desc->isXbox = 0;
desc->type = (std::numeric_limits<uint8_t>::max)();
desc->name[0] = '\0';
desc->axisCount =
HAL_kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */
desc->buttonCount = 0;
desc->povCount = 0;
int retval = FRC_NetworkCommunication_getJoystickDesc(
joystickNum, &desc->isXbox, &desc->type,
reinterpret_cast<char*>(&desc->name), &desc->axisCount,
reinterpret_cast<uint8_t*>(&desc->axisTypes), &desc->buttonCount,
&desc->povCount);
/* check the return, if there is an error and the RIOimage predates FRC2017,
* then axisCount needs to be cleared */
if (retval != 0) {
/* set count to zero so downstream code doesn't decode invalid axisTypes. */
desc->axisCount = 0;
}
return retval;
}
static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
MatchType_t matchType = MatchType_t::kMatchType_none;
info->gameSpecificMessageSize = sizeof(info->gameSpecificMessage);
int status = FRC_NetworkCommunication_getMatchInfo(
info->eventName, &matchType, &info->matchNumber, &info->replayNumber,
info->gameSpecificMessage, &info->gameSpecificMessageSize);
if (info->gameSpecificMessageSize > sizeof(info->gameSpecificMessage)) {
info->gameSpecificMessageSize = 0;
}
info->matchType = static_cast<HAL_MatchType>(matchType);
*(std::end(info->eventName) - 1) = '\0';
return status;
}
namespace {
struct TcpCache {
TcpCache() { std::memset(this, 0, sizeof(*this)); }
bool Update(uint32_t mask);
void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
bool hasReadMatchInfo = false;
HAL_MatchInfo matchInfo;
HAL_JoystickDescriptor descriptors[HAL_kMaxJoysticks];
};
static_assert(std::is_standard_layout_v<TcpCache>);
} // namespace
static std::atomic_uint32_t tcpMask{0xFFFFFFFF};
static TcpCache tcpCache;
static TcpCache tcpCurrent;
static wpi::mutex tcpCacheMutex;
constexpr uint32_t combinedMatchInfoMask = kTcpRecvMask_MatchInfoOld |
kTcpRecvMask_MatchInfo |
kTcpRecvMask_GameSpecific;
bool TcpCache::Update(uint32_t mask) {
bool failedToReadInfo = false;
if ((mask & combinedMatchInfoMask) != 0) {
int status = HAL_GetMatchInfoInternal(&matchInfo);
if (status != 0) {
failedToReadInfo = true;
if (!hasReadMatchInfo) {
std::memset(&matchInfo, 0, sizeof(matchInfo));
}
} else {
hasReadMatchInfo = true;
}
}
for (int i = 0; i < HAL_kMaxJoysticks; i++) {
if ((mask & (1 << i)) != 0) {
HAL_GetJoystickDescriptorInternal(i, &descriptors[i]);
}
}
return failedToReadInfo;
}
namespace hal::init {
void InitializeFRCDriverStation() {
std::memset(&newestControlWord, 0, sizeof(newestControlWord));
static FRCDriverStation ds;
driverStation = &ds;
}
} // namespace hal::init
namespace hal {
static void DefaultPrintErrorImpl(const char* line, size_t size) {
std::fwrite(line, size, 1, stderr);
}
} // namespace hal
static std::atomic<void (*)(const char* line, size_t size)> gPrintErrorImpl{
hal::DefaultPrintErrorImpl};
extern "C" {
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
const char* details, const char* location,
const char* callStack, HAL_Bool printMsg) {
// Avoid flooding console by keeping track of previous 5 error
// messages and only printing again if they're longer than 1 second old.
static constexpr int KEEP_MSGS = 5;
std::scoped_lock lock(msgMutex);
static std::string prevMsg[KEEP_MSGS];
static std::chrono::time_point<std::chrono::steady_clock>
prevMsgTime[KEEP_MSGS];
static bool initialized = false;
if (!initialized) {
for (int i = 0; i < KEEP_MSGS; i++) {
prevMsgTime[i] =
std::chrono::steady_clock::now() - std::chrono::seconds(2);
}
initialized = true;
}
auto curTime = std::chrono::steady_clock::now();
int i;
for (i = 0; i < KEEP_MSGS; ++i) {
if (prevMsg[i] == details) {
break;
}
}
int retval = 0;
if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
std::string_view detailsRef{details};
std::string_view locationRef{location};
std::string_view callStackRef{callStack};
// 2 size, 1 tag, 4 timestamp, 2 seqnum
// 2 numOccur, 4 error code, 1 flags, 6 strlen
// 1 extra needed for padding on Netcomm end.
size_t baseLength = 23;
if (baseLength + detailsRef.size() + locationRef.size() +
callStackRef.size() <=
65535) {
// Pass through
retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
details, location, callStack);
} else if (baseLength + detailsRef.size() > 65535) {
// Details too long, cut both location and stack
auto newLen = 65535 - baseLength;
std::string newDetails{details, newLen};
char empty = '\0';
retval = FRC_NetworkCommunication_sendError(
isError, errorCode, isLVCode, newDetails.c_str(), &empty, &empty);
} else if (baseLength + detailsRef.size() + locationRef.size() > 65535) {
// Location too long, cut stack
auto newLen = 65535 - baseLength - detailsRef.size();
std::string newLocation{location, newLen};
char empty = '\0';
retval = FRC_NetworkCommunication_sendError(
isError, errorCode, isLVCode, details, newLocation.c_str(), &empty);
} else {
// Stack too long
auto newLen = 65535 - baseLength - detailsRef.size() - locationRef.size();
std::string newCallStack{callStack, newLen};
retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
details, location,
newCallStack.c_str());
}
if (printMsg) {
fmt::memory_buffer buf;
if (location && location[0] != '\0') {
fmt::format_to(fmt::appender{buf},
"{} at {}: ", isError ? "Error" : "Warning", location);
}
fmt::format_to(fmt::appender{buf}, "{}\n", details);
if (callStack && callStack[0] != '\0') {
fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
}
auto printError = gPrintErrorImpl.load();
printError(buf.data(), buf.size());
}
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;
}
void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) {
gPrintErrorImpl.store(func ? func : hal::DefaultPrintErrorImpl);
}
int32_t HAL_SendConsoleLine(const char* line) {
std::string_view lineRef{line};
if (lineRef.size() <= 65535) {
// Send directly
return FRC_NetworkCommunication_sendConsoleLine(line);
} else {
// Need to truncate
std::string newLine{line, 65535};
return FRC_NetworkCommunication_sendConsoleLine(newLine.c_str());
}
}
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
std::scoped_lock lock{cacheMutex};
*controlWord = newestControlWord;
return 0;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
return 0;
}
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*povs = currentRead->povs[joystickNum];
return 0;
}
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*buttons = currentRead->buttons[joystickNum];
return 0;
}
void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons) {
std::scoped_lock lock{cacheMutex};
std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes));
std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs));
std::memcpy(buttons, currentRead->buttons, sizeof(currentRead->buttons));
}
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{tcpCacheMutex};
*desc = tcpCurrent.descriptors[joystickNum];
return 0;
}
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
std::scoped_lock lock{tcpCacheMutex};
*info = tcpCurrent.matchInfo;
return 0;
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
std::scoped_lock lock{cacheMutex};
return currentRead->allianceStation;
}
HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return 0;
} else {
return joystickDesc.isXbox;
}
}
int32_t HAL_GetJoystickType(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return -1;
} else {
return joystickDesc.type;
}
}
void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
const char* cName = joystickDesc.name;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
cName = "";
}
auto len = std::strlen(cName);
auto write = WPI_AllocateString(name, len);
std::memcpy(write, cName, len);
}
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) {
CHECK_JOYSTICK_NUMBER(joystickNum);
return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs,
leftRumble, rightRumble);
}
double HAL_GetMatchTime(int32_t* status) {
std::scoped_lock lock{cacheMutex};
return currentRead->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();
}
// Constant number to be used for our occur handle
constexpr int32_t refNumber = 42;
constexpr int32_t tcpRefNumber = 94;
static void tcpOccur(void) {
uint32_t mask = FRC_NetworkCommunication_getNewTcpRecvMask();
tcpMask.fetch_or(mask);
}
static void udpOccur(void) {
cacheToUpdate->Update();
JoystickDataCache* given = cacheToUpdate;
JoystickDataCache* prev = currentCache.exchange(cacheToUpdate);
if (prev == nullptr) {
cacheToUpdate = currentReadLocal;
currentReadLocal = lastGiven;
} else {
// Current read local does not update
cacheToUpdate = prev;
}
lastGiven = given;
driverStation->newDataEvents.Wakeup();
}
static void newDataOccur(uint32_t refNum) {
switch (refNum) {
case refNumber:
udpOccur();
break;
case tcpRefNumber:
tcpOccur();
break;
default:
std::printf("Unknown occur %u\n", refNum);
break;
}
}
HAL_Bool HAL_RefreshDSData(void) {
HAL_ControlWord controlWord;
std::memset(&controlWord, 0, sizeof(controlWord));
FRC_NetworkCommunication_getControlWord(
reinterpret_cast<ControlWord_t*>(&controlWord));
JoystickDataCache* prev;
{
std::scoped_lock lock{cacheMutex};
prev = currentCache.exchange(nullptr);
if (prev != nullptr) {
currentRead = prev;
}
// If newest state shows we have a DS attached, just use the
// control word out of the cache, As it will be the one in sync
// with the data. If no data has been updated, at this point,
// and a DS wasn't attached previously, this will still return
// a zeroed out control word, with is the correct state for
// no new data.
if (!controlWord.dsAttached) {
// If the DS is not attached, we need to zero out the control word.
// This is because HAL_RefreshDSData is called asynchronously from
// the DS data. The dsAttached variable comes directly from netcomm
// and could be updated before the caches are. If that happens,
// we would end up returning the previous cached control word,
// which is out of sync with the current control word and could
// break invariants such as which alliance station is in used.
// Also, when the DS has never been connected the rest of the fields
// in control word are garbage, so we also need to zero out in that
// case too
std::memset(&currentRead->controlWord, 0,
sizeof(currentRead->controlWord));
}
newestControlWord = currentRead->controlWord;
}
uint32_t mask = tcpMask.exchange(0);
if (mask != 0) {
bool failedToReadMatchInfo = tcpCache.Update(mask);
if (failedToReadMatchInfo) {
// If we failed to read match info
// we want to try again next iteration
tcpMask.fetch_or(combinedMatchInfoMask);
}
std::scoped_lock tcpLock(tcpCacheMutex);
tcpCache.CloneTo(&tcpCurrent);
}
return prev != nullptr;
}
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
hal::init::CheckInit();
driverStation->newDataEvents.Add(handle);
}
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
driverStation->newDataEvents.Remove(handle);
}
HAL_Bool HAL_GetOutputsEnabled(void) {
return FRC_NetworkCommunication_getWatchdogActive();
}
} // extern "C"
namespace hal {
void InitializeDriverStation() {
// Set up the occur function internally with NetComm
NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
// Set up our occur reference number
setNewDataOccurRef(refNumber);
FRC_NetworkCommunication_setNewTcpDataOccurRef(tcpRefNumber);
}
void WaitForInitialPacket() {
wpi::Event waitForInitEvent;
driverStation->newDataEvents.Add(waitForInitEvent.GetHandle());
bool timed_out = false;
wpi::WaitForObject(waitForInitEvent.GetHandle(), 0.1, &timed_out);
// Don't care what the result is, just want to give it a chance.
driverStation->newDataEvents.Remove(waitForInitEvent.GetHandle());
}
} // namespace hal

654
hal/src/main/native/athena/HAL.cpp
View File

@@ -1,654 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/HAL.h"
#include <dlfcn.h>
#include <signal.h> // linux for kill
#include <sys/prctl.h>
#include <unistd.h>
#include <atomic>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <memory>
#include <thread>
#include <utility>
#include <FRC_NetworkCommunication/FRCComm.h>
#include <FRC_NetworkCommunication/LoadOut.h>
#include <FRC_NetworkCommunication/UsageReporting.h>
#include <wpi/MemoryBuffer.h>
#include <wpi/SmallString.h>
#include <wpi/StringExtras.h>
#include <wpi/fs.h>
#include <wpi/mutex.h>
#include <wpi/print.h>
#include <wpi/timestamp.h>
#include "FPGACalls.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "hal/ChipObject.h"
#include "hal/DriverStation.h"
#include "hal/Errors.h"
#include "hal/Notifier.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/roborio/HMB.h"
#include "hal/roborio/InterruptManager.h"
#include "visa/visa.h"
using namespace hal;
static std::unique_ptr<tGlobal> global;
static std::unique_ptr<tSysWatchdog> watchdog;
static uint64_t dsStartTime;
static char roboRioCommentsString[64];
static size_t roboRioCommentsStringSize;
static bool roboRioCommentsStringInitialized;
static int32_t teamNumber = -1;
static const volatile HAL_HMBData* hmbBuffer;
#define HAL_HMB_TIMESTAMP_OFFSET 5
using namespace hal;
namespace hal {
void InitializeDriverStation();
void WaitForInitialPacket();
namespace init {
void InitializeHAL() {
InitializeCTREPCM();
InitializeREVPH();
InitializeAddressableLED();
InitializeAccelerometer();
InitializeAnalogAccumulator();
InitializeAnalogGyro();
InitializeAnalogInput();
InitializeAnalogInternal();
InitializeAnalogOutput();
InitializeAnalogTrigger();
InitializeCAN();
InitializeCANAPI();
InitializeConstants();
InitializeCounter();
InitializeDigitalInternal();
InitializeDIO();
InitializeDMA();
InitializeDutyCycle();
InitializeEncoder();
InitializeFPGAEncoder();
InitializeFRCDriverStation();
InitializeI2C();
InitializeInterrupts();
InitializeLEDs();
InitializeMain();
InitializeNotifier();
InitializeCTREPDP();
InitializeREVPDH();
InitializePorts();
InitializePower();
InitializePWM();
InitializeRelay();
InitializeSerialPort();
InitializeSPI();
InitializeThreads();
}
} // namespace init
void ReleaseFPGAInterrupt(int32_t interruptNumber) {
hal::init::CheckInit();
if (!global) {
return;
}
int32_t status = 0;
global->writeInterruptForceNumber(static_cast<unsigned char>(interruptNumber),
&status);
global->strobeInterruptForceOnce(&status);
}
uint64_t GetDSInitializeTime() {
return dsStartTime;
}
} // namespace hal
extern "C" {
HAL_PortHandle HAL_GetPort(int32_t channel) {
// Dont allow a number that wouldn't fit in a uint8_t
if (channel < 0 || channel >= 255) {
return HAL_kInvalidHandle;
}
return createPortHandle(channel, 1);
}
HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) {
// Dont allow a number that wouldn't fit in a uint8_t
if (channel < 0 || channel >= 255) {
return HAL_kInvalidHandle;
}
if (module < 0 || module >= 255) {
return HAL_kInvalidHandle;
}
return createPortHandle(channel, module);
}
const char* HAL_GetErrorMessage(int32_t code) {
switch (code) {
case 0:
return "";
case NiFpga_Status_FifoTimeout:
return NiFpga_Status_FifoTimeout_MESSAGE;
case NiFpga_Status_TransferAborted:
return NiFpga_Status_TransferAborted_MESSAGE;
case NiFpga_Status_MemoryFull:
return NiFpga_Status_MemoryFull_MESSAGE;
case NiFpga_Status_SoftwareFault:
return NiFpga_Status_SoftwareFault_MESSAGE;
case NiFpga_Status_InvalidParameter:
return NiFpga_Status_InvalidParameter_MESSAGE;
case NiFpga_Status_ResourceNotFound:
return NiFpga_Status_ResourceNotFound_MESSAGE;
case NiFpga_Status_ResourceNotInitialized:
return NiFpga_Status_ResourceNotInitialized_MESSAGE;
case NiFpga_Status_HardwareFault:
return NiFpga_Status_HardwareFault_MESSAGE;
case NiFpga_Status_IrqTimeout:
return NiFpga_Status_IrqTimeout_MESSAGE;
case SAMPLE_RATE_TOO_HIGH:
return SAMPLE_RATE_TOO_HIGH_MESSAGE;
case VOLTAGE_OUT_OF_RANGE:
return VOLTAGE_OUT_OF_RANGE_MESSAGE;
case LOOP_TIMING_ERROR:
return LOOP_TIMING_ERROR_MESSAGE;
case SPI_WRITE_NO_MOSI:
return SPI_WRITE_NO_MOSI_MESSAGE;
case SPI_READ_NO_MISO:
return SPI_READ_NO_MISO_MESSAGE;
case SPI_READ_NO_DATA:
return SPI_READ_NO_DATA_MESSAGE;
case INCOMPATIBLE_STATE:
return INCOMPATIBLE_STATE_MESSAGE;
case NO_AVAILABLE_RESOURCES:
return NO_AVAILABLE_RESOURCES_MESSAGE;
case RESOURCE_IS_ALLOCATED:
return RESOURCE_IS_ALLOCATED_MESSAGE;
case RESOURCE_OUT_OF_RANGE:
return RESOURCE_OUT_OF_RANGE_MESSAGE;
case HAL_INVALID_ACCUMULATOR_CHANNEL:
return HAL_INVALID_ACCUMULATOR_CHANNEL_MESSAGE;
case HAL_HANDLE_ERROR:
return HAL_HANDLE_ERROR_MESSAGE;
case NULL_PARAMETER:
return NULL_PARAMETER_MESSAGE;
case ANALOG_TRIGGER_LIMIT_ORDER_ERROR:
return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE;
case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR:
return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE;
case PARAMETER_OUT_OF_RANGE:
return PARAMETER_OUT_OF_RANGE_MESSAGE;
case HAL_COUNTER_NOT_SUPPORTED:
return HAL_COUNTER_NOT_SUPPORTED_MESSAGE;
case HAL_ERR_CANSessionMux_InvalidBuffer:
return ERR_CANSessionMux_InvalidBuffer_MESSAGE;
case HAL_ERR_CANSessionMux_MessageNotFound:
return ERR_CANSessionMux_MessageNotFound_MESSAGE;
case HAL_WARN_CANSessionMux_NoToken:
return WARN_CANSessionMux_NoToken_MESSAGE;
case HAL_ERR_CANSessionMux_NotAllowed:
return ERR_CANSessionMux_NotAllowed_MESSAGE;
case HAL_ERR_CANSessionMux_NotInitialized:
return ERR_CANSessionMux_NotInitialized_MESSAGE;
case VI_ERROR_SYSTEM_ERROR:
return VI_ERROR_SYSTEM_ERROR_MESSAGE;
case VI_ERROR_INV_OBJECT:
return VI_ERROR_INV_OBJECT_MESSAGE;
case VI_ERROR_RSRC_LOCKED:
return VI_ERROR_RSRC_LOCKED_MESSAGE;
case VI_ERROR_RSRC_NFOUND:
return VI_ERROR_RSRC_NFOUND_MESSAGE;
case VI_ERROR_INV_RSRC_NAME:
return VI_ERROR_INV_RSRC_NAME_MESSAGE;
case VI_ERROR_QUEUE_OVERFLOW:
return VI_ERROR_QUEUE_OVERFLOW_MESSAGE;
case VI_ERROR_IO:
return VI_ERROR_IO_MESSAGE;
case VI_ERROR_ASRL_PARITY:
return VI_ERROR_ASRL_PARITY_MESSAGE;
case VI_ERROR_ASRL_FRAMING:
return VI_ERROR_ASRL_FRAMING_MESSAGE;
case VI_ERROR_ASRL_OVERRUN:
return VI_ERROR_ASRL_OVERRUN_MESSAGE;
case VI_ERROR_RSRC_BUSY:
return VI_ERROR_RSRC_BUSY_MESSAGE;
case VI_ERROR_INV_PARAMETER:
return VI_ERROR_INV_PARAMETER_MESSAGE;
case HAL_PWM_SCALE_ERROR:
return HAL_PWM_SCALE_ERROR_MESSAGE;
case HAL_SERIAL_PORT_NOT_FOUND:
return HAL_SERIAL_PORT_NOT_FOUND_MESSAGE;
case HAL_THREAD_PRIORITY_ERROR:
return HAL_THREAD_PRIORITY_ERROR_MESSAGE;
case HAL_THREAD_PRIORITY_RANGE_ERROR:
return HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE;
case HAL_SERIAL_PORT_OPEN_ERROR:
return HAL_SERIAL_PORT_OPEN_ERROR_MESSAGE;
case HAL_SERIAL_PORT_ERROR:
return HAL_SERIAL_PORT_ERROR_MESSAGE;
case HAL_CAN_TIMEOUT:
return HAL_CAN_TIMEOUT_MESSAGE;
case ERR_FRCSystem_NetCommNotResponding:
return ERR_FRCSystem_NetCommNotResponding_MESSAGE;
case ERR_FRCSystem_NoDSConnection:
return ERR_FRCSystem_NoDSConnection_MESSAGE;
case HAL_CAN_BUFFER_OVERRUN:
return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
case HAL_LED_CHANNEL_ERROR:
return HAL_LED_CHANNEL_ERROR_MESSAGE;
case HAL_INVALID_DMA_STATE:
return HAL_INVALID_DMA_STATE_MESSAGE;
case HAL_INVALID_DMA_ADDITION:
return HAL_INVALID_DMA_ADDITION_MESSAGE;
case HAL_USE_LAST_ERROR:
return HAL_USE_LAST_ERROR_MESSAGE;
case HAL_CONSOLE_OUT_ENABLED_ERROR:
return HAL_CONSOLE_OUT_ENABLED_ERROR_MESSAGE;
default:
return "Unknown error status";
}
}
static HAL_RuntimeType runtimeType = HAL_Runtime_RoboRIO;
HAL_RuntimeType HAL_GetRuntimeType(void) {
return runtimeType;
}
int32_t HAL_GetFPGAVersion(int32_t* status) {
hal::init::CheckInit();
if (!global) {
*status = NiFpga_Status_ResourceNotInitialized;
return 0;
}
return global->readVersion(status);
}
int64_t HAL_GetFPGARevision(int32_t* status) {
hal::init::CheckInit();
if (!global) {
*status = NiFpga_Status_ResourceNotInitialized;
return 0;
}
return global->readRevision(status);
}
void HAL_GetSerialNumber(struct WPI_String* serialNumber) {
const char* serialNum = std::getenv("serialnum");
if (!serialNum) {
serialNum = "";
}
size_t len = std::strlen(serialNum);
auto write = WPI_AllocateString(serialNumber, len);
std::memcpy(write, serialNum, len);
}
void InitializeRoboRioComments(void) {
if (!roboRioCommentsStringInitialized) {
auto fileBuffer = wpi::MemoryBuffer::GetFile("/etc/machine-info");
if (!fileBuffer) {
roboRioCommentsStringSize = 0;
roboRioCommentsStringInitialized = true;
return;
}
std::string_view fileContents{
reinterpret_cast<const char*>(fileBuffer.value()->begin()),
fileBuffer.value()->size()};
std::string_view searchString = "PRETTY_HOSTNAME=\"";
size_t start = fileContents.find(searchString);
if (start == std::string_view::npos) {
roboRioCommentsStringSize = 0;
roboRioCommentsStringInitialized = true;
return;
}
start += searchString.size();
std::string_view escapedComments =
wpi::slice(fileContents, start, fileContents.size());
wpi::SmallString<64> buf;
auto [unescapedComments, rem] = wpi::UnescapeCString(escapedComments, buf);
unescapedComments.copy(roboRioCommentsString,
sizeof(roboRioCommentsString));
if (unescapedComments.size() > sizeof(roboRioCommentsString)) {
roboRioCommentsStringSize = sizeof(roboRioCommentsString);
} else {
roboRioCommentsStringSize = unescapedComments.size();
}
roboRioCommentsStringInitialized = true;
}
}
void HAL_GetComments(struct WPI_String* comments) {
if (!roboRioCommentsStringInitialized) {
InitializeRoboRioComments();
}
auto write = WPI_AllocateString(comments, roboRioCommentsStringSize);
std::memcpy(write, roboRioCommentsString, roboRioCommentsStringSize);
}
void InitializeTeamNumber(void) {
char hostnameBuf[25];
auto status = gethostname(hostnameBuf, sizeof(hostnameBuf));
if (status != 0) {
teamNumber = 0;
return;
}
std::string_view hostname{hostnameBuf, sizeof(hostnameBuf)};
// hostname is frc-{TEAM}-roborio
// Split string around '-' (max of 2 splits), take the second element
teamNumber = 0;
int i = 0;
wpi::split(hostname, '-', 2, false, [&](auto part) {
if (i == 1) {
teamNumber = wpi::parse_integer<int32_t>(part, 10).value_or(0);
}
++i;
});
}
int32_t HAL_GetTeamNumber(void) {
if (teamNumber == -1) {
InitializeTeamNumber();
}
return teamNumber;
}
uint64_t HAL_GetFPGATime(int32_t* status) {
hal::init::CheckInit();
if (!hmbBuffer) {
*status = NiFpga_Status_ResourceNotInitialized;
return 0;
}
asm("dmb");
uint64_t upper1 = hmbBuffer->Timestamp.Upper;
asm("dmb");
uint32_t lower = hmbBuffer->Timestamp.Lower;
asm("dmb");
uint64_t upper2 = hmbBuffer->Timestamp.Upper;
if (upper1 != upper2) {
// Rolled over between the lower call, reread lower
asm("dmb");
lower = hmbBuffer->Timestamp.Lower;
}
// 5 is added here because the time to write from the FPGA
// to the HMB buffer is longer then the time to read
// from the time register. This would cause register based
// timestamps to be ahead of HMB timestamps, which could
// be very bad.
return (upper2 << 32) + lower + HAL_HMB_TIMESTAMP_OFFSET;
}
uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) {
// Capture the current FPGA time. This will give us the upper half of the
// clock.
uint64_t fpgaTime = HAL_GetFPGATime(status);
if (*status != 0) {
return 0;
}
// Now, we need to detect the case where the lower bits rolled over after we
// sampled. In that case, the upper bits will be 1 bigger than they should
// be.
// Break it into lower and upper portions.
uint32_t lower = fpgaTime & 0xffffffffull;
uint64_t upper = (fpgaTime >> 32) & 0xffffffff;
// The time was sampled *before* the current time, so roll it back.
if (lower < unexpandedLower) {
--upper;
}
return (upper << 32) + static_cast<uint64_t>(unexpandedLower);
}
HAL_Bool HAL_GetFPGAButton(int32_t* status) {
hal::init::CheckInit();
if (!global) {
*status = NiFpga_Status_ResourceNotInitialized;
return false;
}
return global->readUserButton(status);
}
HAL_Bool HAL_GetSystemActive(int32_t* status) {
hal::init::CheckInit();
if (!watchdog) {
*status = NiFpga_Status_ResourceNotInitialized;
return false;
}
return watchdog->readStatus_SystemActive(status);
}
HAL_Bool HAL_GetBrownedOut(int32_t* status) {
hal::init::CheckInit();
if (!watchdog) {
*status = NiFpga_Status_ResourceNotInitialized;
return false;
}
return !(watchdog->readStatus_PowerAlive(status));
}
int32_t HAL_GetCommsDisableCount(int32_t* status) {
hal::init::CheckInit();
if (!watchdog) {
*status = NiFpga_Status_ResourceNotInitialized;
return 0;
}
return watchdog->readStatus_SysDisableCount(status);
}
HAL_Bool HAL_GetRSLState(int32_t* status) {
hal::init::CheckInit();
if (!global) {
*status = NiFpga_Status_ResourceNotInitialized;
return false;
}
return global->readLEDs_RSL(status);
}
HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
uint8_t timeWasSet = 0;
*status = FRC_NetworkCommunication_getTimeWasSet(&timeWasSet);
return timeWasSet != 0;
}
static bool killExistingProgram(int timeout, int mode) {
// Kill any previous robot programs
std::fstream fs;
// By making this both in/out, it won't give us an error if it doesn't exist
fs.open("/var/lock/frc.pid", std::fstream::in | std::fstream::out);
if (fs.bad()) {
return false;
}
pid_t pid = 0;
if (!fs.eof() && !fs.fail()) {
fs >> pid;
// see if the pid is around, but we don't want to mess with init id=1, or
// ourselves
if (pid >= 2 && kill(pid, 0) == 0 && pid != getpid()) {
std::puts("Killing previously running FRC program...");
kill(pid, SIGTERM); // try to kill it
std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
if (kill(pid, 0) == 0) {
// still not successful
wpi::print(
"FRC pid {} did not die within {} ms. Force killing with kill -9\n",
pid, timeout);
// Force kill -9
auto forceKill = kill(pid, SIGKILL);
if (forceKill != 0) {
auto errorMsg = std::strerror(forceKill);
wpi::print("Kill -9 error: {}\n", errorMsg);
}
// Give a bit of time for the kill to take place
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
}
}
fs.close();
// we will re-open it write only to truncate the file
fs.open("/var/lock/frc.pid", std::fstream::out | std::fstream::trunc);
fs.seekp(0);
pid = getpid();
fs << pid << std::endl;
fs.close();
return true;
}
static bool SetupNowRio(void) {
nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
nLoadOut::getTargetClass();
int32_t status = 0;
Dl_info info;
status = dladdr(reinterpret_cast<void*>(tHMB::create), &info);
if (status == 0) {
wpi::print(stderr, "Failed to call dladdr on chipobject {}\n", dlerror());
return false;
}
void* chipObjectLibrary = dlopen(info.dli_fname, RTLD_LAZY);
if (chipObjectLibrary == nullptr) {
wpi::print(stderr, "Failed to call dlopen on chipobject {}\n", dlerror());
return false;
}
std::unique_ptr<tHMB> hmb;
hmb.reset(tHMB::create(&status));
if (hmb == nullptr) {
wpi::print(stderr, "Failed to open HMB on chipobject {}\n", status);
dlclose(chipObjectLibrary);
return false;
}
return wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb));
}
HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
static std::atomic_bool initialized{false};
static wpi::mutex initializeMutex;
// Initial check, as if it's true initialization has finished
if (initialized) {
return true;
}
std::scoped_lock lock(initializeMutex);
// Second check in case another thread was waiting
if (initialized) {
return true;
}
int fpgaInit = hal::init::InitializeFPGA();
if (fpgaInit != HAL_SUCCESS) {
return false;
}
hal::init::InitializeHAL();
hal::init::HAL_IsInitialized.store(true);
setlinebuf(stdin);
setlinebuf(stdout);
prctl(PR_SET_PDEATHSIG, SIGTERM);
// Return false if program failed to kill an existing program
if (!killExistingProgram(timeout, mode)) {
return false;
}
FRC_NetworkCommunication_Reserve(nullptr);
std::atexit([]() {
// Unregister our new data condition variable.
setNewDataSem(nullptr);
});
if (!SetupNowRio()) {
wpi::print(stderr,
"Failed to run SetupNowRio(). This is a fatal error. The "
"process is being terminated.\n");
std::fflush(stderr);
// Attempt to force a segfault to get a better java log
*reinterpret_cast<int*>(0) = 0;
// If that fails, terminate
std::terminate();
return false;
}
int32_t status = 0;
HAL_InitializeHMB(&status);
if (status != 0) {
wpi::print(stderr, "Failed to open HAL HMB, status code {}\n", status);
return false;
}
hmbBuffer = HAL_GetHMBBuffer();
global.reset(tGlobal::create(&status));
watchdog.reset(tSysWatchdog::create(&status));
if (status != 0) {
return false;
}
nLoadOut::tTargetClass targetClass = nLoadOut::getTargetClass();
if (targetClass == nLoadOut::kTargetClass_RoboRIO2) {
runtimeType = HAL_Runtime_RoboRIO2;
} else {
runtimeType = HAL_Runtime_RoboRIO;
}
InterruptManager::Initialize(global->getSystemInterface());
hal::InitializeDriverStation();
dsStartTime = HAL_GetFPGATime(&status);
if (status != 0) {
return false;
}
hal::WaitForInitialPacket();
initialized = true;
return true;
}
void HAL_Shutdown(void) {}
void HAL_SimPeriodicBefore(void) {}
void HAL_SimPeriodicAfter(void) {}
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);
}
} // extern "C"

14
hal/src/main/native/athena/HALInitializer.cpp
View File

@@ -1,14 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "HALInitializer.h"
#include "hal/HALBase.h"
namespace hal::init {
std::atomic_bool HAL_IsInitialized{false};
void RunInitialize() {
HAL_Initialize(500, 0);
}
} // namespace hal::init

55
hal/src/main/native/athena/HALInitializer.h
View File

@@ -1,55 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <atomic>
namespace hal::init {
extern std::atomic_bool HAL_IsInitialized;
extern void RunInitialize();
inline void CheckInit() {
if (HAL_IsInitialized.load(std::memory_order_relaxed)) {
return;
}
RunInitialize();
}
extern void InitializeCTREPCM();
extern void InitializeREVPH();
extern void InitializeAccelerometer();
extern void InitializeAddressableLED();
extern void InitializeAnalogAccumulator();
extern void InitializeAnalogGyro();
extern void InitializeAnalogInput();
extern void InitializeAnalogInternal();
extern void InitializeAnalogOutput();
extern void InitializeAnalogTrigger();
extern void InitializeCAN();
extern void InitializeCANAPI();
extern void InitializeConstants();
extern void InitializeCounter();
extern void InitializeDigitalInternal();
extern void InitializeDIO();
extern void InitializeDMA();
extern void InitializeDutyCycle();
extern void InitializeEncoder();
extern void InitializeFPGAEncoder();
extern void InitializeFRCDriverStation();
extern void InitializeHAL();
extern void InitializeI2C();
extern void InitializeInterrupts();
extern void InitializeLEDs();
extern void InitializeMain();
extern void InitializeNotifier();
extern void InitializeCTREPDP();
extern void InitializeREVPDH();
extern void InitializePorts();
extern void InitializePower();
extern void InitializePWM();
extern void InitializeRelay();
extern void InitializeSerialPort();
extern void InitializeSPI();
extern void InitializeThreads();
} // namespace hal::init

21
hal/src/main/native/athena/HALInternal.h
View File

@@ -1,21 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
#include <string_view>
namespace hal {
void ReleaseFPGAInterrupt(int32_t interruptNumber);
void SetLastError(int32_t* status, std::string_view value);
void SetLastErrorIndexOutOfRange(int32_t* status, std::string_view message,
int32_t minimum, int32_t maximum,
int32_t channel);
void SetLastErrorPreviouslyAllocated(int32_t* status, std::string_view message,
int32_t channel,
std::string_view previousAllocation);
uint64_t GetDSInitializeTime();
} // namespace hal

82
hal/src/main/native/athena/HMB.cpp
View File

@@ -1,82 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/roborio/HMB.h"
#include <memory>
#include "FPGACalls.h"
#include "hal/ChipObject.h"
using namespace hal;
// 16 classes of data, each takes up 16 uint16_t's
static_assert(sizeof(HAL_HMBData) == 0x10 * 16 * sizeof(uint32_t));
// Timestamp is the last class, and should be offset correctly
static_assert(offsetof(HAL_HMBData, Timestamp) == 0x10 * 15 * sizeof(uint32_t));
static volatile HAL_HMBData* hmbBuffer;
static size_t hmbBufferSize;
static constexpr const char hmbName[] = "HMB_0_RAM";
static std::unique_ptr<tHMB> hmb;
namespace {
struct HMBHolder {
~HMBHolder() {
if (hmbBuffer) {
hal::HAL_NiFpga_CloseHmb(hmb->getSystemInterface()->getHandle(), hmbName);
}
}
};
} // namespace
extern "C" {
void HAL_InitializeHMB(int32_t* status) {
static HMBHolder holder;
hmb.reset(tHMB::create(status));
if (*status != 0) {
return;
}
*status = hal::HAL_NiFpga_OpenHmb(
hmb->getSystemInterface()->getHandle(), hmbName, &hmbBufferSize,
reinterpret_cast<void**>(const_cast<HAL_HMBData**>(&hmbBuffer)));
if (*status != 0) {
return;
}
auto cfg = hmb->readConfig(status);
cfg.Enables_AI0_Low = 1;
cfg.Enables_AI0_High = 1;
cfg.Enables_AIAveraged0_Low = 1;
cfg.Enables_AIAveraged0_High = 1;
cfg.Enables_Accumulator0 = 1;
cfg.Enables_Accumulator1 = 1;
cfg.Enables_DI = 1;
cfg.Enables_AnalogTriggers = 1;
cfg.Enables_Counters_Low = 1;
cfg.Enables_Counters_High = 1;
cfg.Enables_CounterTimers_Low = 1;
cfg.Enables_CounterTimers_High = 1;
cfg.Enables_Encoders_Low = 1;
cfg.Enables_Encoders_High = 1;
cfg.Enables_EncoderTimers_Low = 1;
cfg.Enables_EncoderTimers_High = 1;
cfg.Enables_DutyCycle_Low = 1;
cfg.Enables_DutyCycle_High = 1;
cfg.Enables_Interrupts = 1;
cfg.Enables_PWM = 1;
cfg.Enables_PWM_MXP = 1;
cfg.Enables_Relay_DO_AO = 1;
cfg.Enables_Timestamp = 1;
hmb->writeConfig(cfg, status);
}
const volatile HAL_HMBData* HAL_GetHMBBuffer(void) {
return hmbBuffer;
}
} // extern "C"

210
hal/src/main/native/athena/I2C.cpp
View File

@@ -1,210 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/I2C.h"
#include <fcntl.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <cstring>
#include <wpi/print.h>
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "hal/DIO.h"
#include "hal/HAL.h"
using namespace hal;
static wpi::mutex digitalI2COnBoardMutex;
static wpi::mutex digitalI2CMXPMutex;
static uint8_t i2COnboardObjCount{0};
static uint8_t i2CMXPObjCount{0};
static int i2COnBoardHandle{-1};
static int i2CMXPHandle{-1};
static HAL_DigitalHandle i2CMXPDigitalHandle1{HAL_kInvalidHandle};
static HAL_DigitalHandle i2CMXPDigitalHandle2{HAL_kInvalidHandle};
namespace hal::init {
void InitializeI2C() {}
} // namespace hal::init
extern "C" {
void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) {
hal::init::CheckInit();
initializeDigital(status);
if (*status != 0) {
return;
}
if (port < 0 || port > 1) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for I2C", 0, 1,
port);
return;
}
if (port == HAL_I2C_kOnboard) {
HAL_SendError(0, 0, 0,
"Onboard I2C port is subject to system lockups. See Known "
"Issues page for details",
"", "", true);
std::scoped_lock lock(digitalI2COnBoardMutex);
i2COnboardObjCount++;
if (i2COnboardObjCount > 1) {
return;
}
int handle = open("/dev/i2c-2", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open onboard i2c bus: {}\n", std::strerror(errno));
return;
}
i2COnBoardHandle = handle;
} else {
std::scoped_lock lock(digitalI2CMXPMutex);
i2CMXPObjCount++;
if (i2CMXPObjCount > 1) {
return;
}
if ((i2CMXPDigitalHandle1 = HAL_InitializeDIOPort(
HAL_GetPort(24), false, nullptr, status)) == HAL_kInvalidHandle) {
return;
}
if ((i2CMXPDigitalHandle2 = HAL_InitializeDIOPort(
HAL_GetPort(25), false, nullptr, status)) == HAL_kInvalidHandle) {
HAL_FreeDIOPort(i2CMXPDigitalHandle1); // free the first port allocated
return;
}
digitalSystem->writeEnableMXPSpecialFunction(
digitalSystem->readEnableMXPSpecialFunction(status) | 0xC000, status);
int handle = open("/dev/i2c-1", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open MXP i2c bus: {}\n", std::strerror(errno));
return;
}
i2CMXPHandle = handle;
}
}
int32_t HAL_TransactionI2C(HAL_I2CPort port, int32_t deviceAddress,
const uint8_t* dataToSend, int32_t sendSize,
uint8_t* dataReceived, int32_t receiveSize) {
if (port < 0 || port > 1) {
int32_t status = 0;
hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
port);
return -1;
}
struct i2c_msg msgs[2];
msgs[0].addr = deviceAddress;
msgs[0].flags = 0;
msgs[0].len = sendSize;
msgs[0].buf = const_cast<uint8_t*>(dataToSend);
msgs[1].addr = deviceAddress;
msgs[1].flags = I2C_M_RD;
msgs[1].len = receiveSize;
msgs[1].buf = dataReceived;
struct i2c_rdwr_ioctl_data rdwr;
rdwr.msgs = msgs;
rdwr.nmsgs = 2;
if (port == HAL_I2C_kOnboard) {
std::scoped_lock lock(digitalI2COnBoardMutex);
return ioctl(i2COnBoardHandle, I2C_RDWR, &rdwr);
} else {
std::scoped_lock lock(digitalI2CMXPMutex);
return ioctl(i2CMXPHandle, I2C_RDWR, &rdwr);
}
}
int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress,
const uint8_t* dataToSend, int32_t sendSize) {
if (port < 0 || port > 1) {
int32_t status = 0;
hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
port);
return -1;
}
struct i2c_msg msg;
msg.addr = deviceAddress;
msg.flags = 0;
msg.len = sendSize;
msg.buf = const_cast<uint8_t*>(dataToSend);
struct i2c_rdwr_ioctl_data rdwr;
rdwr.msgs = &msg;
rdwr.nmsgs = 1;
if (port == HAL_I2C_kOnboard) {
std::scoped_lock lock(digitalI2COnBoardMutex);
return ioctl(i2COnBoardHandle, I2C_RDWR, &rdwr);
} else {
std::scoped_lock lock(digitalI2CMXPMutex);
return ioctl(i2CMXPHandle, I2C_RDWR, &rdwr);
}
}
int32_t HAL_ReadI2C(HAL_I2CPort port, int32_t deviceAddress, uint8_t* buffer,
int32_t count) {
if (port < 0 || port > 1) {
int32_t status = 0;
hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
port);
return -1;
}
struct i2c_msg msg;
msg.addr = deviceAddress;
msg.flags = I2C_M_RD;
msg.len = count;
msg.buf = buffer;
struct i2c_rdwr_ioctl_data rdwr;
rdwr.msgs = &msg;
rdwr.nmsgs = 1;
if (port == HAL_I2C_kOnboard) {
std::scoped_lock lock(digitalI2COnBoardMutex);
return ioctl(i2COnBoardHandle, I2C_RDWR, &rdwr);
} else {
std::scoped_lock lock(digitalI2CMXPMutex);
return ioctl(i2CMXPHandle, I2C_RDWR, &rdwr);
}
}
void HAL_CloseI2C(HAL_I2CPort port) {
if (port < 0 || port > 1) {
int32_t status = 0;
hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
port);
return;
}
if (port == HAL_I2C_kOnboard) {
std::scoped_lock lock(digitalI2COnBoardMutex);
if (i2COnboardObjCount-- == 0) {
close(i2COnBoardHandle);
}
} else {
std::scoped_lock lock(digitalI2CMXPMutex);
if (i2CMXPObjCount-- == 0) {
close(i2CMXPHandle);
}
HAL_FreeDIOPort(i2CMXPDigitalHandle1);
HAL_FreeDIOPort(i2CMXPDigitalHandle2);
}
}
} // extern "C"

72
hal/src/main/native/athena/InterruptManager.cpp
View File

@@ -1,72 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/roborio/InterruptManager.h"
#include <fmt/format.h>
#include "FPGACalls.h"
#include "HALInternal.h"
#include "dlfcn.h"
#include "hal/Errors.h"
using namespace hal;
InterruptManager& InterruptManager::GetInstance() {
static InterruptManager manager;
return manager;
}
void InterruptManager::Initialize(tSystemInterface* baseSystem) {
auto& manager = GetInstance();
manager.fpgaSession = baseSystem->getHandle();
}
NiFpga_IrqContext InterruptManager::GetContext() noexcept {
NiFpga_IrqContext context;
HAL_NiFpga_ReserveIrqContext(fpgaSession, &context);
return context;
}
void InterruptManager::ReleaseContext(NiFpga_IrqContext context) noexcept {
HAL_NiFpga_UnreserveIrqContext(fpgaSession, context);
}
uint32_t InterruptManager::WaitForInterrupt(NiFpga_IrqContext context,
uint32_t mask, bool ignorePrevious,
uint32_t timeoutMs,
int32_t* status) {
{
// Make sure we can safely use this
std::scoped_lock lock(currentMaskMutex);
if ((currentMask & mask) != 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status, fmt::format("Interrupt mask {} has bits {} already in use",
mask, (currentMask & mask)));
return 0;
}
currentMask |= mask;
}
if (ignorePrevious) {
HAL_NiFpga_AcknowledgeIrqs(fpgaSession, mask);
}
uint32_t irqsAsserted = 0;
NiFpga_Bool timedOut = 0;
*status = HAL_NiFpga_WaitOnIrqs(fpgaSession, context, mask, timeoutMs,
&irqsAsserted, &timedOut);
if (!timedOut) {
HAL_NiFpga_AcknowledgeIrqs(fpgaSession, irqsAsserted);
}
{
std::scoped_lock lock(currentMaskMutex);
currentMask &= ~mask;
}
return irqsAsserted;
}

199
hal/src/main/native/athena/Interrupts.cpp
View File

@@ -1,199 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Interrupts.h"
#include <memory>
#include <wpi/SafeThread.h>
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/ChipObject.h"
#include "hal/Errors.h"
#include "hal/HALBase.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
#include "hal/roborio/InterruptManager.h"
using namespace hal;
namespace {
struct Interrupt {
std::unique_ptr<tInterrupt> anInterrupt;
InterruptManager& manager = InterruptManager::GetInstance();
NiFpga_IrqContext irqContext = nullptr;
uint32_t mask;
};
} // namespace
static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
HAL_HandleEnum::Interrupt>* interruptHandles;
namespace hal::init {
void InitializeInterrupts() {
static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
HAL_HandleEnum::Interrupt>
iH;
interruptHandles = &iH;
}
} // namespace hal::init
extern "C" {
HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status) {
hal::init::CheckInit();
HAL_InterruptHandle handle = interruptHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto anInterrupt = interruptHandles->Get(handle);
uint32_t interruptIndex = static_cast<uint32_t>(getHandleIndex(handle));
// Expects the calling leaf class to allocate an interrupt index.
anInterrupt->anInterrupt.reset(tInterrupt::create(interruptIndex, status));
anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
anInterrupt->irqContext = anInterrupt->manager.GetContext();
anInterrupt->mask = (1u << interruptIndex) | (1u << (interruptIndex + 8u));
return handle;
}
void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle) {
auto anInterrupt = interruptHandles->Get(interruptHandle);
interruptHandles->Free(interruptHandle);
if (anInterrupt == nullptr) {
return;
}
if (anInterrupt->irqContext) {
anInterrupt->manager.ReleaseContext(anInterrupt->irqContext);
}
}
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.WaitForInterrupt(
anInterrupt->irqContext, anInterrupt->mask, ignorePrevious,
static_cast<uint32_t>(timeout * 1e3), 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;
}
int64_t HAL_WaitForMultipleInterrupts(HAL_InterruptHandle interruptHandle,
int64_t mask, 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.WaitForInterrupt(
anInterrupt->irqContext, mask, ignorePrevious,
static_cast<uint32_t>(timeout * 1e3), 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;
}
int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
int32_t* status) {
auto anInterrupt = interruptHandles->Get(interruptHandle);
if (anInterrupt == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
uint32_t timestamp = anInterrupt->anInterrupt->readRisingTimeStamp(status);
return timestamp;
}
int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
int32_t* status) {
auto anInterrupt = interruptHandles->Get(interruptHandle);
if (anInterrupt == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
uint32_t timestamp = anInterrupt->anInterrupt->readFallingTimeStamp(status);
return timestamp;
}
void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
int32_t* status) {
auto anInterrupt = interruptHandles->Get(interruptHandle);
if (anInterrupt == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
bool routingAnalogTrigger = false;
uint8_t routingChannel = 0;
uint8_t routingModule = 0;
bool success =
remapDigitalSource(digitalSourceHandle, analogTriggerType, routingChannel,
routingModule, routingAnalogTrigger);
if (!success) {
*status = HAL_HANDLE_ERROR;
return;
}
anInterrupt->anInterrupt->writeConfig_Source_AnalogTrigger(
routingAnalogTrigger, status);
anInterrupt->anInterrupt->writeConfig_Source_Channel(routingChannel, status);
anInterrupt->anInterrupt->writeConfig_Source_Module(routingModule, status);
}
void HAL_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);
}
void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
int32_t* status) {
auto anInterrupt = interruptHandles->Get(interruptHandle);
if (anInterrupt == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint32_t interruptIndex =
static_cast<uint32_t>(getHandleIndex(interruptHandle));
hal::ReleaseFPGAInterrupt(interruptIndex);
hal::ReleaseFPGAInterrupt(interruptIndex + 8);
}
} // extern "C"

112
hal/src/main/native/athena/LEDs.cpp
View File

@@ -1,112 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/LEDs.h"
#include <unistd.h>
#include <cstring>
#include <fstream>
#include <fmt/format.h>
#include <fmt/std.h>
#include <wpi/fs.h>
#include "HALInternal.h"
#include "hal/Errors.h"
namespace hal::init {
void InitializeLEDs() {
int32_t status = 0;
HAL_SetRadioLEDState(HAL_RadioLED_kOff, &status);
}
} // namespace hal::init
static const fs::path radioLEDGreenFilePath =
"/sys/class/leds/nilrt:wifi:primary/brightness";
static const fs::path radioLEDRedFilePath =
"/sys/class/leds/nilrt:wifi:secondary/brightness";
static const char* onStr = "1";
static const char* offStr = "0";
static bool ReadStateFromFile(fs::path path, int32_t* status) {
std::error_code ec;
fs::file_t file = fs::OpenFileForRead(path, ec, fs::OF_Text);
if (ec) {
hal::SetLastError(status, fmt::format("Could not open '{}' for read: {}",
path, ec.message()));
*status = INCOMPATIBLE_STATE;
return false;
}
// We only need to read one byte because the file won't have leading zeros.
char buf[1]{};
ssize_t count = read(file, buf, 1);
// save errno, always close file.
int err = errno;
fs::CloseFile(file);
if (count <= 0) {
*status = INCOMPATIBLE_STATE;
if (count == 0) {
hal::SetLastError(status,
fmt::format("Read from '{}' returned no data.", path));
} else {
hal::SetLastError(status, fmt::format("Failed to read from '{}': {}",
path, std::strerror(err)));
}
return false;
}
// If the brightness is not zero, the LED is on.
return buf[0] != '0';
}
extern "C" {
void HAL_SetRadioLEDState(HAL_RadioLEDState state, int32_t* status) {
std::error_code ec;
fs::file_t greenFile = fs::OpenFileForWrite(radioLEDGreenFilePath, ec,
fs::CD_OpenExisting, fs::OF_Text);
if (ec) {
// not opened, nothing to clean up
*status = INCOMPATIBLE_STATE;
hal::SetLastError(status, fmt::format("Could not open '{}' for write: {}",
greenFile, ec.message()));
return;
}
fs::file_t redFile = fs::OpenFileForWrite(radioLEDRedFilePath, ec,
fs::CD_OpenExisting, fs::OF_Text);
if (ec) {
// green file opened successfully, need to close it
fs::CloseFile(greenFile);
*status = INCOMPATIBLE_STATE;
hal::SetLastError(status, fmt::format("Could not open '{}' for write: {}",
greenFile, ec.message()));
return;
}
write(greenFile, state & HAL_RadioLED_kGreen ? onStr : offStr, 1);
write(redFile, state & HAL_RadioLED_kRed ? onStr : offStr, 1);
fs::CloseFile(greenFile);
fs::CloseFile(redFile);
}
HAL_RadioLEDState HAL_GetRadioLEDState(int32_t* status) {
bool green = ReadStateFromFile(radioLEDGreenFilePath, status);
bool red = ReadStateFromFile(radioLEDRedFilePath, status);
if (*status == 0) {
if (green && red) {
return HAL_RadioLED_kOrange;
} else if (green) {
return HAL_RadioLED_kGreen;
} else if (red) {
return HAL_RadioLED_kRed;
} else {
return HAL_RadioLED_kOff;
}
} else {
return HAL_RadioLED_kOff;
}
}
} // extern "C"

309
hal/src/main/native/athena/Notifier.cpp
View File

@@ -1,309 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Notifier.h"
#include <atomic>
#include <cstdlib> // For std::atexit()
#include <memory>
#include <thread>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/print.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "hal/ChipObject.h"
#include "hal/Errors.h"
#include "hal/HAL.h"
#include "hal/Threads.h"
#include "hal/handles/UnlimitedHandleResource.h"
#include "hal/roborio/InterruptManager.h"
using namespace hal;
static constexpr int32_t kTimerInterruptNumber = 28;
static wpi::mutex notifierMutex;
static std::unique_ptr<tAlarm> notifierAlarm;
static std::thread notifierThread;
static HAL_Bool notifierThreadRealTime = false;
static int32_t notifierThreadPriority = 0;
static uint64_t closestTrigger{UINT64_MAX};
namespace {
struct Notifier {
uint64_t triggerTime = UINT64_MAX;
uint64_t triggeredTime = UINT64_MAX;
bool active = true;
wpi::mutex mutex;
wpi::condition_variable cond;
};
} // namespace
static std::atomic_flag notifierAtexitRegistered{ATOMIC_FLAG_INIT};
static std::atomic_int notifierRefCount{0};
static std::atomic_bool notifierRunning{false};
using namespace hal;
class NotifierHandleContainer
: public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier> {
public:
~NotifierHandleContainer() {
ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
{
std::scoped_lock lock(notifier->mutex);
notifier->triggerTime = UINT64_MAX;
notifier->triggeredTime = 0;
notifier->active = false;
}
notifier->cond.notify_all(); // wake up any waiting threads
});
}
};
static NotifierHandleContainer* notifierHandles;
static void alarmCallback() {
std::scoped_lock lock(notifierMutex);
int32_t status = 0;
uint64_t currentTime = 0;
// the hardware disables itself after each alarm
closestTrigger = UINT64_MAX;
// process all notifiers
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
if (notifier->triggerTime == UINT64_MAX) {
return;
}
if (currentTime == 0) {
currentTime = HAL_GetFPGATime(&status);
}
std::unique_lock lock(notifier->mutex);
if (notifier->triggerTime < currentTime) {
notifier->triggerTime = UINT64_MAX;
notifier->triggeredTime = currentTime;
lock.unlock();
notifier->cond.notify_all();
} else if (notifier->triggerTime < closestTrigger) {
closestTrigger = notifier->triggerTime;
}
});
if (notifierAlarm && closestTrigger != UINT64_MAX) {
// Simply truncate the hardware trigger time to 32-bit.
notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
&status);
// Enable the alarm. The hardware disables itself after each alarm.
notifierAlarm->writeEnable(true, &status);
}
}
static void notifierThreadMain() {
InterruptManager& manager = InterruptManager::GetInstance();
NiFpga_IrqContext context = manager.GetContext();
uint32_t mask = 1 << kTimerInterruptNumber;
int32_t status = 0;
while (notifierRunning) {
status = 0;
auto triggeredMask =
manager.WaitForInterrupt(context, mask, false, 10000, &status);
if (!notifierRunning) {
break;
}
if (triggeredMask == 0) {
continue;
}
alarmCallback();
}
}
static void cleanupNotifierAtExit() {
int32_t status = 0;
if (notifierAlarm) {
notifierAlarm->writeEnable(false, &status);
}
notifierAlarm = nullptr;
notifierRunning = false;
hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
if (notifierThread.joinable()) {
notifierThread.join();
}
}
namespace hal::init {
void InitializeNotifier() {
static NotifierHandleContainer nH;
notifierHandles = &nH;
}
} // namespace hal::init
extern "C" {
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
hal::init::CheckInit();
if (!notifierAtexitRegistered.test_and_set()) {
std::atexit(cleanupNotifierAtExit);
}
if (notifierRefCount.fetch_add(1) == 0) {
std::scoped_lock lock(notifierMutex);
notifierRunning = true;
notifierThread = std::thread(notifierThreadMain);
auto native = notifierThread.native_handle();
HAL_SetThreadPriority(&native, notifierThreadRealTime,
notifierThreadPriority, status);
if (*status == HAL_THREAD_PRIORITY_ERROR) {
*status = 0;
wpi::print("{}: HAL Notifier thread\n",
HAL_THREAD_PRIORITY_ERROR_MESSAGE);
}
if (*status == HAL_THREAD_PRIORITY_RANGE_ERROR) {
*status = 0;
wpi::print("{}: HAL Notifier thread\n",
HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE);
}
notifierAlarm.reset(tAlarm::create(status));
}
std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
if (handle == HAL_kInvalidHandle) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
return handle;
}
HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
int32_t* status) {
std::scoped_lock lock(notifierMutex);
notifierThreadRealTime = realTime;
notifierThreadPriority = priority;
if (notifierThread.joinable()) {
auto native = notifierThread.native_handle();
return HAL_SetThreadPriority(&native, realTime, priority, status);
} else {
return true;
}
}
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status) {}
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->triggerTime = UINT64_MAX;
notifier->triggeredTime = 0;
notifier->active = false;
}
notifier->cond.notify_all(); // wake up any waiting threads
}
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) {
auto notifier = notifierHandles->Free(notifierHandle);
if (!notifier) {
return;
}
// Just in case HAL_StopNotifier() wasn't called...
{
std::scoped_lock lock(notifier->mutex);
notifier->triggerTime = UINT64_MAX;
notifier->triggeredTime = 0;
notifier->active = false;
}
notifier->cond.notify_all();
if (notifierRefCount.fetch_sub(1) == 1) {
// if this was the last notifier, clean up alarm and thread
// the notifier can call back into our callback, so don't hold the lock
// here (the atomic fetch_sub will prevent multiple parallel entries
// into this function)
int32_t status = 0;
if (notifierAlarm) {
notifierAlarm->writeEnable(false, &status);
}
notifierRunning = false;
hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
if (notifierThread.joinable()) {
notifierThread.join();
}
std::scoped_lock lock(notifierMutex);
notifierAlarm = nullptr;
closestTrigger = UINT64_MAX;
}
}
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->triggerTime = triggerTime;
notifier->triggeredTime = UINT64_MAX;
}
std::scoped_lock lock(notifierMutex);
// Update alarm time if closer than current.
if (triggerTime < closestTrigger) {
bool wasActive = (closestTrigger != UINT64_MAX);
closestTrigger = triggerTime;
// Simply truncate the hardware trigger time to 32-bit.
notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
status);
// Enable the alarm.
if (!wasActive) {
notifierAlarm->writeEnable(true, status);
}
}
}
void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->triggerTime = UINT64_MAX;
}
}
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return 0;
}
std::unique_lock lock(notifier->mutex);
notifier->cond.wait(lock, [&] {
return !notifier->active || notifier->triggeredTime != UINT64_MAX;
});
return notifier->active ? notifier->triggeredTime : 0;
}
} // extern "C"

468
hal/src/main/native/athena/PWM.cpp
View File

@@ -1,468 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/PWM.h"
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <thread>
#include <fmt/format.h>
#include <wpi/print.h>
#include "ConstantsInternal.h"
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/cpp/fpga_clock.h"
#include "hal/handles/HandlesInternal.h"
using namespace hal;
static inline int32_t GetMaxPositivePwm(DigitalPort* port) {
return port->maxPwm;
}
static inline int32_t GetMinPositivePwm(DigitalPort* port) {
if (port->eliminateDeadband) {
return port->deadbandMaxPwm;
} else {
return port->centerPwm + 1;
}
}
static inline int32_t GetCenterPwm(DigitalPort* port) {
return port->centerPwm;
}
static inline int32_t GetMaxNegativePwm(DigitalPort* port) {
if (port->eliminateDeadband) {
return port->deadbandMinPwm;
} else {
return port->centerPwm - 1;
}
}
static inline int32_t GetMinNegativePwm(DigitalPort* port) {
return port->minPwm;
}
static inline int32_t GetPositiveScaleFactor(DigitalPort* port) {
return GetMaxPositivePwm(port) - GetMinPositivePwm(port);
} ///< The scale for positive speeds.
static inline int32_t GetNegativeScaleFactor(DigitalPort* port) {
return GetMaxNegativePwm(port) - GetMinNegativePwm(port);
} ///< The scale for negative speeds.
static inline int32_t GetFullRangeScaleFactor(DigitalPort* port) {
return GetMaxPositivePwm(port) - GetMinNegativePwm(port);
} ///< The scale for positions.
namespace hal::init {
void InitializePWM() {}
} // namespace hal::init
extern "C" {
HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
initializeDigital(status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
int16_t channel = getPortHandleChannel(portHandle);
if (channel == InvalidHandleIndex || channel >= kNumPWMChannels) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
kNumPWMChannels, channel);
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
}
HAL_DigitalHandle handle;
auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM,
&handle, status);
if (*status != 0) {
if (port) {
hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", origChannel,
port->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
kNumPWMChannels, origChannel);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
port->channel = origChannel;
if (port->channel > tPWM::kNumHdrRegisters - 1) {
int32_t bitToSet = 1 << remapMXPPWMChannel(port->channel);
uint16_t specialFunctions =
digitalSystem->readEnableMXPSpecialFunction(status);
digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet,
status);
}
// Defaults to allow an always valid config.
HAL_SetPWMConfigMicroseconds(handle, 2000, 1501, 1500, 1499, 1000, status);
port->previousAllocation = allocationLocation ? allocationLocation : "";
return handle;
}
void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
return;
}
digitalChannelHandles->Free(pwmPortHandle, HAL_HandleEnum::PWM);
// Wait for no other object to hold this handle.
auto start = hal::fpga_clock::now();
while (port.use_count() != 1) {
auto current = hal::fpga_clock::now();
if (start + std::chrono::seconds(1) < current) {
std::puts("PWM handle free timeout");
std::fflush(stdout);
break;
}
std::this_thread::yield();
}
if (port->channel > tPWM::kNumHdrRegisters - 1) {
int32_t status = 0;
int32_t bitToUnset = 1 << remapMXPPWMChannel(port->channel);
uint16_t specialFunctions =
digitalSystem->readEnableMXPSpecialFunction(&status);
digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset,
&status);
if (status != 0) {
wpi::println(
"HAL_FreePWMPort: failed to free MXP PWM port {}, status code {}",
port->channel, status);
}
}
}
HAL_Bool HAL_CheckPWMChannel(int32_t channel) {
return channel < kNumPWMChannels && channel >= 0;
}
void HAL_SetPWMConfigMicroseconds(HAL_DigitalHandle pwmPortHandle, int32_t max,
int32_t deadbandMax, int32_t center,
int32_t deadbandMin, int32_t min,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
port->maxPwm = max;
port->deadbandMaxPwm = deadbandMax;
port->deadbandMinPwm = deadbandMin;
port->centerPwm = center;
port->minPwm = min;
port->configSet = true;
}
void HAL_GetPWMConfigMicroseconds(HAL_DigitalHandle pwmPortHandle,
int32_t* maxPwm, int32_t* deadbandMaxPwm,
int32_t* centerPwm, int32_t* deadbandMinPwm,
int32_t* minPwm, int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
*maxPwm = port->maxPwm;
*deadbandMaxPwm = port->deadbandMaxPwm;
*deadbandMinPwm = port->deadbandMinPwm;
*centerPwm = port->centerPwm;
*minPwm = port->minPwm;
}
void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
HAL_Bool eliminateDeadband, int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
port->eliminateDeadband = eliminateDeadband;
}
HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
return port->eliminateDeadband;
}
void HAL_SetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle,
int32_t microsecondPulseTime,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (microsecondPulseTime < 0 ||
(microsecondPulseTime != 0xFFFF && microsecondPulseTime >= 4096)) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Pulse time {} out of range. Expect [0-4096) or 0xFFFF",
microsecondPulseTime));
return;
}
if (port->channel < tPWM::kNumHdrRegisters) {
pwmSystem->writeHdr(port->channel, microsecondPulseTime, status);
} else {
pwmSystem->writeMXP(port->channel - tPWM::kNumHdrRegisters,
microsecondPulseTime, status);
}
}
void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (!port->configSet) {
*status = INCOMPATIBLE_STATE;
return;
}
DigitalPort* dPort = port.get();
if (std::isfinite(speed)) {
speed = std::clamp(speed, -1.0, 1.0);
} else {
speed = 0.0;
}
// calculate the desired output pwm value by scaling the speed appropriately
int32_t rawValue;
if (speed == 0.0) {
rawValue = GetCenterPwm(dPort);
} else if (speed > 0.0) {
rawValue =
std::lround(speed * static_cast<double>(GetPositiveScaleFactor(dPort)) +
static_cast<double>(GetMinPositivePwm(dPort)));
} else {
rawValue =
std::lround(speed * static_cast<double>(GetNegativeScaleFactor(dPort)) +
static_cast<double>(GetMaxNegativePwm(dPort)));
}
if (!((rawValue >= GetMinNegativePwm(dPort)) &&
(rawValue <= GetMaxPositivePwm(dPort))) ||
rawValue == kPwmDisabled) {
*status = HAL_PWM_SCALE_ERROR;
return;
}
HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, rawValue, status);
}
void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double pos,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (!port->configSet) {
*status = INCOMPATIBLE_STATE;
return;
}
DigitalPort* dPort = port.get();
if (pos < 0.0) {
pos = 0.0;
} else if (pos > 1.0) {
pos = 1.0;
}
// note, need to perform the multiplication below as floating point before
// converting to int
int32_t rawValue = static_cast<int32_t>(
(pos * static_cast<double>(GetFullRangeScaleFactor(dPort))) +
GetMinNegativePwm(dPort));
if (rawValue == kPwmDisabled) {
*status = HAL_PWM_SCALE_ERROR;
return;
}
HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, rawValue, status);
}
void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, kPwmDisabled, status);
}
int32_t HAL_GetPWMPulseTimeMicroseconds(HAL_DigitalHandle pwmPortHandle,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (port->channel < tPWM::kNumHdrRegisters) {
return pwmSystem->readHdr(port->channel, status);
} else {
return pwmSystem->readMXP(port->channel - tPWM::kNumHdrRegisters, status);
}
}
double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (!port->configSet) {
*status = INCOMPATIBLE_STATE;
return 0;
}
int32_t value = HAL_GetPWMPulseTimeMicroseconds(pwmPortHandle, status);
if (*status != 0) {
return 0;
}
DigitalPort* dPort = port.get();
if (value == kPwmDisabled) {
return 0.0;
} else if (value > GetMaxPositivePwm(dPort)) {
return 1.0;
} else if (value < GetMinNegativePwm(dPort)) {
return -1.0;
} else if (value > GetMinPositivePwm(dPort)) {
return static_cast<double>(value - GetMinPositivePwm(dPort)) /
static_cast<double>(GetPositiveScaleFactor(dPort));
} else if (value < GetMaxNegativePwm(dPort)) {
return static_cast<double>(value - GetMaxNegativePwm(dPort)) /
static_cast<double>(GetNegativeScaleFactor(dPort));
} else {
return 0.0;
}
}
double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (!port->configSet) {
*status = INCOMPATIBLE_STATE;
return 0;
}
int32_t value = HAL_GetPWMPulseTimeMicroseconds(pwmPortHandle, status);
if (*status != 0) {
return 0;
}
DigitalPort* dPort = port.get();
if (value < GetMinNegativePwm(dPort)) {
return 0.0;
} else if (value > GetMaxPositivePwm(dPort)) {
return 1.0;
} else {
return static_cast<double>(value - GetMinNegativePwm(dPort)) /
static_cast<double>(GetFullRangeScaleFactor(dPort));
}
}
void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
pwmSystem->writeZeroLatch(port->channel, true, status);
pwmSystem->writeZeroLatch(port->channel, false, status);
}
void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
int32_t* status) {
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (port->channel < tPWM::kNumPeriodScaleHdrElements) {
pwmSystem->writePeriodScaleHdr(port->channel, squelchMask, status);
} else {
pwmSystem->writePeriodScaleMXP(
port->channel - tPWM::kNumPeriodScaleHdrElements, squelchMask, status);
}
}
void HAL_SetPWMAlwaysHighMode(HAL_DigitalHandle pwmPortHandle,
int32_t* status) {
HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, kPwmAlwaysHigh, status);
}
int32_t HAL_GetPWMLoopTiming(int32_t* status) {
initializeDigital(status);
if (*status != 0) {
return 0;
}
return pwmSystem->readLoopTiming(status);
}
uint64_t HAL_GetPWMCycleStartTime(int32_t* status) {
initializeDigital(status);
if (*status != 0) {
return 0;
}
uint64_t upper1 = pwmSystem->readCycleStartTimeUpper(status);
uint32_t lower = pwmSystem->readCycleStartTime(status);
uint64_t upper2 = pwmSystem->readCycleStartTimeUpper(status);
if (*status != 0) {
return 0;
}
if (upper1 != upper2) {
// Rolled over between the lower call, reread lower
lower = pwmSystem->readCycleStartTime(status);
if (*status != 0) {
return 0;
}
}
return (upper2 << 32) + lower;
}
} // extern "C"

90
hal/src/main/native/athena/Ports.cpp
View File

@@ -1,90 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Ports.h"
#include "PortsInternal.h"
using namespace hal;
namespace hal::init {
void InitializePorts() {}
} // namespace hal::init
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_GetNumCTREPCMModules(void) {
return kNumCTREPCMModules;
}
int32_t HAL_GetNumCTRESolenoidChannels(void) {
return kNumCTRESolenoidChannels;
}
int32_t HAL_GetNumCTREPDPModules(void) {
return kNumCTREPDPModules;
}
int32_t HAL_GetNumCTREPDPChannels(void) {
return kNumCTREPDPChannels;
}
int32_t HAL_GetNumREVPDHModules(void) {
return kNumREVPDHModules;
}
int32_t HAL_GetNumREVPDHChannels(void) {
return kNumREVPDHChannels;
}
int32_t HAL_GetNumREVPHModules(void) {
return kNumREVPHModules;
}
int32_t HAL_GetNumREVPHChannels(void) {
return kNumREVPHChannels;
}
int32_t HAL_GetNumDutyCycles(void) {
return kNumDutyCycles;
}
int32_t HAL_GetNumAddressableLEDs(void) {
return kNumAddressableLEDs;
}
} // extern "C"

43
hal/src/main/native/athena/PortsInternal.h
View File

@@ -1,43 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this 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 =
static_cast<int32_t>(tDIO::kNumPWMDutyCycleAElements) +
static_cast<int32_t>(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 kNumCTREPCMModules = 63;
constexpr int32_t kNumCTRESolenoidChannels = 8;
constexpr int32_t kNumCTREPDPModules = 63;
constexpr int32_t kNumCTREPDPChannels = 16;
constexpr int32_t kNumREVPDHModules = 63;
constexpr int32_t kNumREVPDHChannels = 24;
constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems;
constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems;
constexpr int32_t kNumREVPHModules = 63;
constexpr int32_t kNumREVPHChannels = 16;
} // namespace hal

149
hal/src/main/native/athena/Power.cpp
View File

@@ -1,149 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Power.h"
#include <memory>
#include "HALInitializer.h"
#include "hal/ChipObject.h"
using namespace hal;
namespace hal {
static std::unique_ptr<tPower> power{nullptr};
static void initializePower(int32_t* status) {
hal::init::CheckInit();
if (power == nullptr) {
power.reset(tPower::create(status));
}
}
} // namespace hal
namespace hal::init {
void InitializePower() {}
} // namespace hal::init
extern "C" {
double HAL_GetVinVoltage(int32_t* status) {
initializePower(status);
return power->readVinVoltage(status) / 4.096 * 0.025733 - 0.029;
}
double HAL_GetVinCurrent(int32_t* status) {
initializePower(status);
return power->readVinCurrent(status) / 4.096 * 0.017042 - 0.071;
}
double HAL_GetUserVoltage6V(int32_t* status) {
initializePower(status);
return power->readUserVoltage6V(status) / 4.096 * 0.007019 - 0.014;
}
double HAL_GetUserCurrent6V(int32_t* status) {
initializePower(status);
return power->readUserCurrent6V(status) / 4.096 * 0.005566 - 0.009;
}
HAL_Bool HAL_GetUserActive6V(int32_t* status) {
initializePower(status);
return power->readStatus_User6V(status) == 4;
}
int32_t HAL_GetUserCurrentFaults6V(int32_t* status) {
initializePower(status);
return static_cast<int32_t>(
power->readFaultCounts_OverCurrentFaultCount6V(status));
}
void HAL_SetUserRailEnabled6V(HAL_Bool enabled, int32_t* status) {
initializePower(status);
power->writeDisable_User6V(!enabled, status);
}
double HAL_GetUserVoltage5V(int32_t* status) {
initializePower(status);
return power->readUserVoltage5V(status) / 4.096 * 0.005962 - 0.013;
}
double HAL_GetUserCurrent5V(int32_t* status) {
initializePower(status);
return power->readUserCurrent5V(status) / 4.096 * 0.001996 - 0.002;
}
HAL_Bool HAL_GetUserActive5V(int32_t* status) {
initializePower(status);
return power->readStatus_User5V(status) == 4;
}
int32_t HAL_GetUserCurrentFaults5V(int32_t* status) {
initializePower(status);
return static_cast<int32_t>(
power->readFaultCounts_OverCurrentFaultCount5V(status));
}
void HAL_SetUserRailEnabled5V(HAL_Bool enabled, int32_t* status) {
initializePower(status);
power->writeDisable_User5V(!enabled, status);
}
double HAL_GetUserVoltage3V3(int32_t* status) {
initializePower(status);
return power->readUserVoltage3V3(status) / 4.096 * 0.004902 - 0.01;
}
double HAL_GetUserCurrent3V3(int32_t* status) {
initializePower(status);
return power->readUserCurrent3V3(status) / 4.096 * 0.002486 - 0.003;
}
HAL_Bool HAL_GetUserActive3V3(int32_t* status) {
initializePower(status);
return power->readStatus_User3V3(status) == 4;
}
int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
initializePower(status);
return static_cast<int32_t>(
power->readFaultCounts_OverCurrentFaultCount3V3(status));
}
void HAL_SetUserRailEnabled3V3(HAL_Bool enabled, int32_t* status) {
initializePower(status);
power->writeDisable_User3V3(!enabled, status);
}
void HAL_ResetUserCurrentFaults(int32_t* status) {
initializePower(status);
power->strobeResetFaultCounts(status);
}
void HAL_SetBrownoutVoltage(double voltage, int32_t* status) {
initializePower(status);
if (voltage < 0) {
voltage = 0;
}
if (voltage > 50) {
voltage = 50;
}
power->writeBrownoutVoltage250mV(static_cast<unsigned char>(voltage * 4),
status);
}
double HAL_GetBrownoutVoltage(int32_t* status) {
initializePower(status);
auto brownout = power->readBrownoutVoltage250mV(status);
return brownout / 4.0;
}
double HAL_GetCPUTemp(int32_t* status) {
initializePower(status);
return power->readOnChipTemperature(status) / 4096.0 * 503.975 - 273.15;
}
} // extern "C"

308
hal/src/main/native/athena/PowerDistribution.cpp
View File

@@ -1,308 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/PowerDistribution.h"
#include <cstring>
#include <thread>
#include "CTREPDP.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "REVPDH.h"
#include "hal/Errors.h"
#include "hal/HALBase.h"
#include "hal/handles/HandlesInternal.h"
using namespace hal;
extern "C" {
HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
int32_t moduleNumber, HAL_PowerDistributionType type,
const char* allocationLocation, int32_t* status) {
if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic) {
if (moduleNumber != HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status, "Automatic PowerDistributionType must have default module");
return HAL_kInvalidHandle;
}
uint64_t waitTime = hal::GetDSInitializeTime() + 400000;
// Ensure we have been alive for long enough to receive a few Power packets.
do {
uint64_t currentTime = HAL_GetFPGATime(status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
if (currentTime >= waitTime) {
break;
}
std::this_thread::sleep_for(
std::chrono::microseconds(waitTime - currentTime));
} while (true);
// Try PDP first
auto pdpHandle = HAL_InitializePDP(0, allocationLocation, status);
if (pdpHandle != HAL_kInvalidHandle) {
*status = 0;
HAL_GetPDPVoltage(pdpHandle, status);
if (*status == 0 || *status == HAL_CAN_TIMEOUT) {
return static_cast<HAL_PowerDistributionHandle>(pdpHandle);
}
HAL_CleanPDP(pdpHandle);
}
*status = 0;
auto pdhHandle = HAL_InitializeREVPDH(1, allocationLocation, status);
return static_cast<HAL_PowerDistributionHandle>(pdhHandle);
}
if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
moduleNumber = 0;
}
return static_cast<HAL_PowerDistributionHandle>(
HAL_InitializePDP(moduleNumber, allocationLocation, status));
} else {
if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
moduleNumber = 1;
}
return static_cast<HAL_PowerDistributionHandle>(
HAL_InitializeREVPDH(moduleNumber, allocationLocation, status));
}
}
#define IsCtre(handle) ::hal::isHandleType(handle, HAL_HandleEnum::CTREPDP)
void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) {
if (IsCtre(handle)) {
HAL_CleanPDP(handle);
} else {
HAL_FreeREVPDH(handle);
}
}
int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPModuleNumber(handle, status);
} else {
return HAL_GetREVPDHModuleNumber(handle, status);
}
}
HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle,
int32_t channel) {
if (IsCtre(handle)) {
return HAL_CheckPDPChannel(channel);
} else {
return HAL_CheckREVPDHChannelNumber(channel);
}
}
HAL_Bool HAL_CheckPowerDistributionModule(int32_t module,
HAL_PowerDistributionType type) {
if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
return HAL_CheckPDPModule(module);
} else {
return HAL_CheckREVPDHModuleNumber(module);
}
}
HAL_PowerDistributionType HAL_GetPowerDistributionType(
HAL_PowerDistributionHandle handle, int32_t* status) {
return IsCtre(handle)
? HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE
: HAL_PowerDistributionType::HAL_PowerDistributionType_kRev;
}
int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return kNumCTREPDPChannels;
} else {
return kNumREVPDHChannels;
}
}
double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPTemperature(handle, status);
} else {
// Not supported
return 0;
}
}
double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPVoltage(handle, status);
} else {
return HAL_GetREVPDHVoltage(handle, status);
}
}
double HAL_GetPowerDistributionChannelCurrent(
HAL_PowerDistributionHandle handle, int32_t channel, int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPChannelCurrent(handle, channel, status);
} else {
return HAL_GetREVPDHChannelCurrent(handle, channel, status);
}
}
void HAL_GetPowerDistributionAllChannelCurrents(
HAL_PowerDistributionHandle handle, double* currents,
int32_t currentsLength, int32_t* status) {
if (IsCtre(handle)) {
if (currentsLength < kNumCTREPDPChannels) {
*status = PARAMETER_OUT_OF_RANGE;
SetLastError(status, "Output array not large enough");
return;
}
return HAL_GetPDPAllChannelCurrents(handle, currents, status);
} else {
if (currentsLength < kNumREVPDHChannels) {
*status = PARAMETER_OUT_OF_RANGE;
SetLastError(status, "Output array not large enough");
return;
}
return HAL_GetREVPDHAllChannelCurrents(handle, currents, status);
}
}
double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPTotalCurrent(handle, status);
} else {
return HAL_GetREVPDHTotalCurrent(handle, status);
}
}
double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPTotalPower(handle, status);
} else {
// Not currently supported
return 0;
}
}
double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPTotalEnergy(handle, status);
} else {
// Not currently supported
return 0;
}
}
void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
HAL_ResetPDPTotalEnergy(handle, status);
} else {
// Not supported
}
}
void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
HAL_ClearPDPStickyFaults(handle, status);
} else {
HAL_ClearREVPDHStickyFaults(handle, status);
}
}
void HAL_SetPowerDistributionSwitchableChannel(
HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status) {
if (IsCtre(handle)) {
// No-op on CTRE
return;
} else {
HAL_SetREVPDHSwitchableChannel(handle, enabled, status);
}
}
HAL_Bool HAL_GetPowerDistributionSwitchableChannel(
HAL_PowerDistributionHandle handle, int32_t* status) {
if (IsCtre(handle)) {
// No-op on CTRE
return false;
} else {
return HAL_GetREVPDHSwitchableChannelState(handle, status);
}
}
void HAL_GetPowerDistributionVersion(HAL_PowerDistributionHandle handle,
HAL_PowerDistributionVersion* version,
int32_t* status) {
if (IsCtre(handle)) {
std::memset(version, 0, sizeof(*version));
} else {
HAL_GetREVPDHVersion(handle, version, status);
}
}
void HAL_GetPowerDistributionFaults(HAL_PowerDistributionHandle handle,
HAL_PowerDistributionFaults* faults,
int32_t* status) {
if (IsCtre(handle)) {
std::memset(faults, 0, sizeof(*faults));
} else {
HAL_GetREVPDHFaults(handle, faults, status);
}
}
void HAL_GetPowerDistributionStickyFaults(
HAL_PowerDistributionHandle handle,
HAL_PowerDistributionStickyFaults* stickyFaults, int32_t* status) {
if (IsCtre(handle)) {
std::memset(stickyFaults, 0, sizeof(*stickyFaults));
} else {
HAL_GetREVPDHStickyFaults(handle, stickyFaults, status);
}
}
void HAL_StartPowerDistributionStream(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
HAL_StartPDPStream(handle, status);
} else {
HAL_StartREVPDHStream(handle, status);
}
}
HAL_PowerDistributionChannelData* HAL_GetPowerDistributionStreamData(
HAL_PowerDistributionHandle handle, int32_t* count, int32_t* status) {
if (IsCtre(handle)) {
return HAL_GetPDPStreamData(handle, count, status);
} else {
return HAL_GetREVPDHStreamData(handle, count, status);
}
}
void HAL_FreePowerDistributionStreamData(HAL_PowerDistributionChannelData* data,
int32_t count) {
delete[] data;
}
void HAL_StopPowerDistributionStream(HAL_PowerDistributionHandle handle,
int32_t* status) {
if (IsCtre(handle)) {
HAL_StopPDPStream(handle, status);
} else {
HAL_StopREVPDHStream(handle, status);
}
}
} // extern "C"

915
hal/src/main/native/athena/REVPDH.cpp
View File

@@ -1,915 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "REVPDH.h"
#include <hal/CAN.h>
#include <hal/CANAPI.h>
#include <hal/CANAPITypes.h>
#include <hal/Errors.h>
#include <hal/handles/HandlesInternal.h>
#include <hal/handles/IndexedHandleResource.h>
#include <cstring>
#include <string>
#include <thread>
#include <fmt/format.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "rev/PDHFrames.h"
using namespace hal;
static constexpr HAL_CANManufacturer manufacturer =
HAL_CANManufacturer::HAL_CAN_Man_kREV;
static constexpr HAL_CANDeviceType deviceType =
HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
static constexpr int32_t kDefaultControlPeriod = 50;
namespace {
struct REV_PDHObj {
int32_t controlPeriod;
HAL_CANHandle hcan;
std::string previousAllocation;
HAL_PowerDistributionVersion versionInfo;
bool streamHandleAllocated{false};
uint32_t streamSessionHandles[4];
};
} // namespace
static constexpr uint32_t APIFromExtId(uint32_t extId) {
return (extId >> 6) & 0x3FF;
}
static constexpr uint32_t PDH_SET_SWITCH_CHANNEL_FRAME_API =
APIFromExtId(PDH_SET_SWITCH_CHANNEL_FRAME_ID);
static constexpr uint32_t PDH_STATUS_0_FRAME_API =
APIFromExtId(PDH_STATUS_0_FRAME_ID);
static constexpr uint32_t PDH_STATUS_1_FRAME_API =
APIFromExtId(PDH_STATUS_1_FRAME_ID);
static constexpr uint32_t PDH_STATUS_2_FRAME_API =
APIFromExtId(PDH_STATUS_2_FRAME_ID);
static constexpr uint32_t PDH_STATUS_3_FRAME_API =
APIFromExtId(PDH_STATUS_3_FRAME_ID);
static constexpr uint32_t PDH_STATUS_4_FRAME_API =
APIFromExtId(PDH_STATUS_4_FRAME_ID);
static constexpr uint32_t PDH_CLEAR_FAULTS_FRAME_API =
APIFromExtId(PDH_CLEAR_FAULTS_FRAME_ID);
static constexpr uint32_t PDH_VERSION_FRAME_API =
APIFromExtId(PDH_VERSION_FRAME_ID);
static constexpr int32_t kPDHFrameStatus0Timeout = 20;
static constexpr int32_t kPDHFrameStatus1Timeout = 20;
static constexpr int32_t kPDHFrameStatus2Timeout = 20;
static constexpr int32_t kPDHFrameStatus3Timeout = 20;
static constexpr int32_t kPDHFrameStatus4Timeout = 20;
static IndexedHandleResource<HAL_REVPDHHandle, REV_PDHObj, kNumREVPDHModules,
HAL_HandleEnum::REVPDH>* REVPDHHandles;
namespace hal::init {
void InitializeREVPDH() {
static IndexedHandleResource<HAL_REVPDHHandle, REV_PDHObj, kNumREVPDHModules,
HAL_HandleEnum::REVPDH>
rH;
REVPDHHandles = &rH;
}
} // namespace hal::init
extern "C" {
static PDH_status_0_t HAL_ReadREVPDHStatus0(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_status_0_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_0_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus0Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_0_unpack(&result, packedData, PDH_STATUS_0_LENGTH);
return result;
}
static PDH_status_1_t HAL_ReadREVPDHStatus1(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_status_1_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_1_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus1Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_1_unpack(&result, packedData, PDH_STATUS_1_LENGTH);
return result;
}
static PDH_status_2_t HAL_ReadREVPDHStatus2(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_status_2_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_2_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus2Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_2_unpack(&result, packedData, PDH_STATUS_2_LENGTH);
return result;
}
static PDH_status_3_t HAL_ReadREVPDHStatus3(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_status_3_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_3_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus3Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_3_unpack(&result, packedData, PDH_STATUS_3_LENGTH);
return result;
}
static PDH_status_4_t HAL_ReadREVPDHStatus4(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_status_4_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_4_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus4Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_4_unpack(&result, packedData, PDH_STATUS_4_LENGTH);
return result;
}
/**
* Helper function for the individual getter functions for status 4
*/
PDH_status_4_t HAL_GetREVPDHStatus4(HAL_REVPDHHandle handle, int32_t* status) {
PDH_status_4_t statusFrame = {};
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return statusFrame;
}
statusFrame = HAL_ReadREVPDHStatus4(hpdh->hcan, status);
return statusFrame;
}
HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
if (!HAL_CheckREVPDHModuleNumber(module)) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 1,
kNumREVPDHModules, module);
return HAL_kInvalidHandle;
}
HAL_REVPDHHandle handle;
// Module starts at 1
auto hpdh = REVPDHHandles->Allocate(module - 1, &handle, status);
if (*status != 0) {
if (hpdh) {
hal::SetLastErrorPreviouslyAllocated(status, "REV PDH", module,
hpdh->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 1,
kNumREVPDHModules, module);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
HAL_CANHandle hcan =
HAL_InitializeCAN(manufacturer, module, deviceType, status);
if (*status != 0) {
REVPDHHandles->Free(handle);
return HAL_kInvalidHandle;
}
hpdh->previousAllocation = allocationLocation ? allocationLocation : "";
hpdh->hcan = hcan;
hpdh->controlPeriod = kDefaultControlPeriod;
std::memset(&hpdh->versionInfo, 0, sizeof(hpdh->versionInfo));
return handle;
}
void HAL_FreeREVPDH(HAL_REVPDHHandle handle) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
return;
}
HAL_CleanCAN(hpdh->hcan);
REVPDHHandles->Free(handle);
}
int32_t HAL_GetREVPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status) {
return hal::getHandleIndex(handle);
}
HAL_Bool HAL_CheckREVPDHModuleNumber(int32_t module) {
return ((module >= 1) && (module <= kNumREVPDHModules)) ? 1 : 0;
}
HAL_Bool HAL_CheckREVPDHChannelNumber(int32_t channel) {
return ((channel >= 0) && (channel < kNumREVPDHChannels)) ? 1 : 0;
}
double HAL_GetREVPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel,
int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (!HAL_CheckREVPDHChannelNumber(channel)) {
*status = RESOURCE_OUT_OF_RANGE;
return 0;
}
// Determine what periodic status the channel is in
if (channel < 6) {
// Periodic status 0
PDH_status_0_t statusFrame = HAL_ReadREVPDHStatus0(hpdh->hcan, status);
switch (channel) {
case 0:
return PDH_status_0_channel_0_current_decode(
statusFrame.channel_0_current);
case 1:
return PDH_status_0_channel_1_current_decode(
statusFrame.channel_1_current);
case 2:
return PDH_status_0_channel_2_current_decode(
statusFrame.channel_2_current);
case 3:
return PDH_status_0_channel_3_current_decode(
statusFrame.channel_3_current);
case 4:
return PDH_status_0_channel_4_current_decode(
statusFrame.channel_4_current);
case 5:
return PDH_status_0_channel_5_current_decode(
statusFrame.channel_5_current);
}
} else if (channel < 12) {
// Periodic status 1
PDH_status_1_t statusFrame = HAL_ReadREVPDHStatus1(hpdh->hcan, status);
switch (channel) {
case 6:
return PDH_status_1_channel_6_current_decode(
statusFrame.channel_6_current);
case 7:
return PDH_status_1_channel_7_current_decode(
statusFrame.channel_7_current);
case 8:
return PDH_status_1_channel_8_current_decode(
statusFrame.channel_8_current);
case 9:
return PDH_status_1_channel_9_current_decode(
statusFrame.channel_9_current);
case 10:
return PDH_status_1_channel_10_current_decode(
statusFrame.channel_10_current);
case 11:
return PDH_status_1_channel_11_current_decode(
statusFrame.channel_11_current);
}
} else if (channel < 18) {
// Periodic status 2
PDH_status_2_t statusFrame = HAL_ReadREVPDHStatus2(hpdh->hcan, status);
switch (channel) {
case 12:
return PDH_status_2_channel_12_current_decode(
statusFrame.channel_12_current);
case 13:
return PDH_status_2_channel_13_current_decode(
statusFrame.channel_13_current);
case 14:
return PDH_status_2_channel_14_current_decode(
statusFrame.channel_14_current);
case 15:
return PDH_status_2_channel_15_current_decode(
statusFrame.channel_15_current);
case 16:
return PDH_status_2_channel_16_current_decode(
statusFrame.channel_16_current);
case 17:
return PDH_status_2_channel_17_current_decode(
statusFrame.channel_17_current);
}
} else if (channel < 24) {
// Periodic status 3
PDH_status_3_t statusFrame = HAL_ReadREVPDHStatus3(hpdh->hcan, status);
switch (channel) {
case 18:
return PDH_status_3_channel_18_current_decode(
statusFrame.channel_18_current);
case 19:
return PDH_status_3_channel_19_current_decode(
statusFrame.channel_19_current);
case 20:
return PDH_status_3_channel_20_current_decode(
statusFrame.channel_20_current);
case 21:
return PDH_status_3_channel_21_current_decode(
statusFrame.channel_21_current);
case 22:
return PDH_status_3_channel_22_current_decode(
statusFrame.channel_22_current);
case 23:
return PDH_status_3_channel_23_current_decode(
statusFrame.channel_23_current);
}
}
return 0;
}
void HAL_GetREVPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents,
int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PDH_status_0_t statusFrame0 = HAL_ReadREVPDHStatus0(hpdh->hcan, status);
PDH_status_1_t statusFrame1 = HAL_ReadREVPDHStatus1(hpdh->hcan, status);
PDH_status_2_t statusFrame2 = HAL_ReadREVPDHStatus2(hpdh->hcan, status);
PDH_status_3_t statusFrame3 = HAL_ReadREVPDHStatus3(hpdh->hcan, status);
currents[0] =
PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current);
currents[1] =
PDH_status_0_channel_1_current_decode(statusFrame0.channel_1_current);
currents[2] =
PDH_status_0_channel_2_current_decode(statusFrame0.channel_2_current);
currents[3] =
PDH_status_0_channel_3_current_decode(statusFrame0.channel_3_current);
currents[4] =
PDH_status_0_channel_4_current_decode(statusFrame0.channel_4_current);
currents[5] =
PDH_status_0_channel_5_current_decode(statusFrame0.channel_5_current);
currents[6] =
PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current);
currents[7] =
PDH_status_1_channel_7_current_decode(statusFrame1.channel_7_current);
currents[8] =
PDH_status_1_channel_8_current_decode(statusFrame1.channel_8_current);
currents[9] =
PDH_status_1_channel_9_current_decode(statusFrame1.channel_9_current);
currents[10] =
PDH_status_1_channel_10_current_decode(statusFrame1.channel_10_current);
currents[11] =
PDH_status_1_channel_11_current_decode(statusFrame1.channel_11_current);
currents[12] =
PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current);
currents[13] =
PDH_status_2_channel_13_current_decode(statusFrame2.channel_13_current);
currents[14] =
PDH_status_2_channel_14_current_decode(statusFrame2.channel_14_current);
currents[15] =
PDH_status_2_channel_15_current_decode(statusFrame2.channel_15_current);
currents[16] =
PDH_status_2_channel_16_current_decode(statusFrame2.channel_16_current);
currents[17] =
PDH_status_2_channel_17_current_decode(statusFrame2.channel_17_current);
currents[18] =
PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current);
currents[19] =
PDH_status_3_channel_19_current_decode(statusFrame3.channel_19_current);
currents[20] =
PDH_status_3_channel_20_current_decode(statusFrame3.channel_20_current);
currents[21] =
PDH_status_3_channel_21_current_decode(statusFrame3.channel_21_current);
currents[22] =
PDH_status_3_channel_22_current_decode(statusFrame3.channel_22_current);
currents[23] =
PDH_status_3_channel_23_current_decode(statusFrame3.channel_23_current);
}
uint16_t HAL_GetREVPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status) {
PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status);
if (*status != 0) {
return 0;
}
return PDH_status_4_total_current_decode(statusFrame.total_current);
}
void HAL_SetREVPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled,
int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t packedData[8] = {0};
PDH_set_switch_channel_t frame;
frame.output_set_value = enabled;
PDH_set_switch_channel_pack(packedData, &frame,
PDH_SET_SWITCH_CHANNEL_LENGTH);
HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_SET_SWITCH_CHANNEL_LENGTH,
PDH_SET_SWITCH_CHANNEL_FRAME_API, status);
}
HAL_Bool HAL_GetREVPDHSwitchableChannelState(HAL_REVPDHHandle handle,
int32_t* status) {
PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status);
if (*status != 0) {
return 0.0;
}
return PDH_status_4_switch_channel_state_decode(
statusFrame.switch_channel_state);
}
double HAL_GetREVPDHVoltage(HAL_REVPDHHandle handle, int32_t* status) {
PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status);
if (*status != 0) {
return 0.0;
}
return PDH_status_4_v_bus_decode(statusFrame.v_bus);
}
void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
HAL_PowerDistributionVersion* version,
int32_t* status) {
std::memset(version, 0, sizeof(*version));
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_version_t result = {};
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (hpdh->versionInfo.firmwareMajor > 0) {
version->firmwareMajor = hpdh->versionInfo.firmwareMajor;
version->firmwareMinor = hpdh->versionInfo.firmwareMinor;
version->firmwareFix = hpdh->versionInfo.firmwareFix;
version->hardwareMajor = hpdh->versionInfo.hardwareMajor;
version->hardwareMinor = hpdh->versionInfo.hardwareMinor;
version->uniqueId = hpdh->versionInfo.uniqueId;
*status = 0;
return;
}
HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API,
status);
if (*status != 0) {
return;
}
uint32_t timeoutMs = 100;
for (uint32_t i = 0; i <= timeoutMs; i++) {
HAL_ReadCANPacketNew(hpdh->hcan, PDH_VERSION_FRAME_API, packedData, &length,
&timestamp, status);
if (*status == 0) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
if (*status != 0) {
return;
}
PDH_version_unpack(&result, packedData, PDH_VERSION_LENGTH);
version->firmwareMajor = result.firmware_year;
version->firmwareMinor = result.firmware_minor;
version->firmwareFix = result.firmware_fix;
version->hardwareMinor = result.hardware_minor;
version->hardwareMajor = result.hardware_major;
version->uniqueId = result.unique_id;
hpdh->versionInfo = *version;
}
void HAL_GetREVPDHFaults(HAL_REVPDHHandle handle,
HAL_PowerDistributionFaults* faults, int32_t* status) {
std::memset(faults, 0, sizeof(*faults));
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PDH_status_0_t status0 = HAL_ReadREVPDHStatus0(hpdh->hcan, status);
PDH_status_1_t status1 = HAL_ReadREVPDHStatus1(hpdh->hcan, status);
PDH_status_2_t status2 = HAL_ReadREVPDHStatus2(hpdh->hcan, status);
PDH_status_3_t status3 = HAL_ReadREVPDHStatus3(hpdh->hcan, status);
PDH_status_4_t status4 = HAL_ReadREVPDHStatus4(hpdh->hcan, status);
faults->channel0BreakerFault = status0.channel_0_breaker_fault;
faults->channel1BreakerFault = status0.channel_1_breaker_fault;
faults->channel2BreakerFault = status0.channel_2_breaker_fault;
faults->channel3BreakerFault = status0.channel_3_breaker_fault;
faults->channel4BreakerFault = status1.channel_4_breaker_fault;
faults->channel5BreakerFault = status1.channel_5_breaker_fault;
faults->channel6BreakerFault = status1.channel_6_breaker_fault;
faults->channel7BreakerFault = status1.channel_7_breaker_fault;
faults->channel8BreakerFault = status2.channel_8_breaker_fault;
faults->channel9BreakerFault = status2.channel_9_breaker_fault;
faults->channel10BreakerFault = status2.channel_10_breaker_fault;
faults->channel11BreakerFault = status2.channel_11_breaker_fault;
faults->channel12BreakerFault = status3.channel_12_breaker_fault;
faults->channel13BreakerFault = status3.channel_13_breaker_fault;
faults->channel14BreakerFault = status3.channel_14_breaker_fault;
faults->channel15BreakerFault = status3.channel_15_breaker_fault;
faults->channel16BreakerFault = status3.channel_16_breaker_fault;
faults->channel17BreakerFault = status3.channel_17_breaker_fault;
faults->channel18BreakerFault = status3.channel_18_breaker_fault;
faults->channel19BreakerFault = status3.channel_19_breaker_fault;
faults->channel20BreakerFault = status3.channel_20_breaker_fault;
faults->channel21BreakerFault = status3.channel_21_breaker_fault;
faults->channel22BreakerFault = status3.channel_22_breaker_fault;
faults->channel23BreakerFault = status3.channel_23_breaker_fault;
faults->brownout = status4.brownout_fault;
faults->canWarning = status4.can_warning_fault;
faults->hardwareFault = status4.hardware_fault;
}
void HAL_GetREVPDHStickyFaults(HAL_REVPDHHandle handle,
HAL_PowerDistributionStickyFaults* stickyFaults,
int32_t* status) {
std::memset(stickyFaults, 0, sizeof(*stickyFaults));
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PDH_status_4_t status4 = HAL_ReadREVPDHStatus4(hpdh->hcan, status);
stickyFaults->channel0BreakerFault = status4.sticky_ch0_breaker_fault;
stickyFaults->channel1BreakerFault = status4.sticky_ch1_breaker_fault;
stickyFaults->channel2BreakerFault = status4.sticky_ch2_breaker_fault;
stickyFaults->channel3BreakerFault = status4.sticky_ch3_breaker_fault;
stickyFaults->channel4BreakerFault = status4.sticky_ch4_breaker_fault;
stickyFaults->channel5BreakerFault = status4.sticky_ch5_breaker_fault;
stickyFaults->channel6BreakerFault = status4.sticky_ch6_breaker_fault;
stickyFaults->channel7BreakerFault = status4.sticky_ch7_breaker_fault;
stickyFaults->channel8BreakerFault = status4.sticky_ch8_breaker_fault;
stickyFaults->channel9BreakerFault = status4.sticky_ch9_breaker_fault;
stickyFaults->channel10BreakerFault = status4.sticky_ch10_breaker_fault;
stickyFaults->channel11BreakerFault = status4.sticky_ch11_breaker_fault;
stickyFaults->channel12BreakerFault = status4.sticky_ch12_breaker_fault;
stickyFaults->channel13BreakerFault = status4.sticky_ch13_breaker_fault;
stickyFaults->channel14BreakerFault = status4.sticky_ch14_breaker_fault;
stickyFaults->channel15BreakerFault = status4.sticky_ch15_breaker_fault;
stickyFaults->channel16BreakerFault = status4.sticky_ch16_breaker_fault;
stickyFaults->channel17BreakerFault = status4.sticky_ch17_breaker_fault;
stickyFaults->channel18BreakerFault = status4.sticky_ch18_breaker_fault;
stickyFaults->channel19BreakerFault = status4.sticky_ch19_breaker_fault;
stickyFaults->channel20BreakerFault = status4.sticky_ch20_breaker_fault;
stickyFaults->channel21BreakerFault = status4.sticky_ch21_breaker_fault;
stickyFaults->channel22BreakerFault = status4.sticky_ch22_breaker_fault;
stickyFaults->channel23BreakerFault = status4.sticky_ch23_breaker_fault;
stickyFaults->brownout = status4.sticky_brownout_fault;
stickyFaults->canWarning = status4.sticky_can_warning_fault;
stickyFaults->canBusOff = status4.sticky_can_bus_off_fault;
stickyFaults->hardwareFault = status4.sticky_hardware_fault;
stickyFaults->firmwareFault = status4.sticky_firmware_fault;
stickyFaults->hasReset = status4.sticky_has_reset_fault;
}
void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t packedData[8] = {0};
HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_CLEAR_FAULTS_LENGTH,
PDH_CLEAR_FAULTS_FRAME_API, status);
}
uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
int32_t* status);
void HAL_StartREVPDHStream(HAL_REVPDHHandle handle, int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (hpdh->streamHandleAllocated) {
*status = RESOURCE_IS_ALLOCATED;
return;
}
hpdh->streamSessionHandles[0] =
HAL_StartCANStream(hpdh->hcan, PDH_STATUS_0_FRAME_API, 50, status);
if (*status != 0) {
return;
}
hpdh->streamSessionHandles[1] =
HAL_StartCANStream(hpdh->hcan, PDH_STATUS_1_FRAME_API, 50, status);
if (*status != 0) {
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
return;
}
hpdh->streamSessionHandles[2] =
HAL_StartCANStream(hpdh->hcan, PDH_STATUS_2_FRAME_API, 50, status);
if (*status != 0) {
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]);
return;
}
hpdh->streamSessionHandles[3] =
HAL_StartCANStream(hpdh->hcan, PDH_STATUS_3_FRAME_API, 50, status);
if (*status != 0) {
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]);
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[3]);
return;
}
hpdh->streamHandleAllocated = true;
}
HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
HAL_REVPDHHandle handle, int32_t* count, int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return nullptr;
}
if (!hpdh->streamHandleAllocated) {
*status = RESOURCE_OUT_OF_RANGE;
return nullptr;
}
*count = 0;
// 4 streams, 6 channels per stream, 50 depth per stream
HAL_PowerDistributionChannelData* retData =
new HAL_PowerDistributionChannelData[4 * 6 * 50];
HAL_CANStreamMessage messages[50];
uint32_t messagesRead = 0;
HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[0], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_0_t statusFrame0;
PDH_status_0_unpack(&statusFrame0, messages[i].data, PDH_STATUS_0_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current);
retData[*count].channel = 1;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_0_channel_1_current_decode(statusFrame0.channel_1_current);
retData[*count].channel = 2;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_0_channel_2_current_decode(statusFrame0.channel_2_current);
retData[*count].channel = 3;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_0_channel_3_current_decode(statusFrame0.channel_3_current);
retData[*count].channel = 4;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_0_channel_4_current_decode(statusFrame0.channel_4_current);
retData[*count].channel = 5;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_0_channel_5_current_decode(statusFrame0.channel_5_current);
retData[*count].channel = 6;
retData[*count].timestamp = timestamp;
(*count)++;
}
messagesRead = 0;
HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[1], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_1_t statusFrame1;
PDH_status_1_unpack(&statusFrame1, messages[i].data, PDH_STATUS_1_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current);
retData[*count].channel = 7;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_1_channel_7_current_decode(statusFrame1.channel_7_current);
retData[*count].channel = 8;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_1_channel_8_current_decode(statusFrame1.channel_8_current);
retData[*count].channel = 9;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_1_channel_9_current_decode(statusFrame1.channel_9_current);
retData[*count].channel = 10;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_1_channel_10_current_decode(statusFrame1.channel_10_current);
retData[*count].channel = 11;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_1_channel_11_current_decode(statusFrame1.channel_11_current);
retData[*count].channel = 12;
retData[*count].timestamp = timestamp;
(*count)++;
}
messagesRead = 0;
HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[2], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_2_t statusFrame2;
PDH_status_2_unpack(&statusFrame2, messages[i].data, PDH_STATUS_2_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current);
retData[*count].channel = 13;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_2_channel_13_current_decode(statusFrame2.channel_13_current);
retData[*count].channel = 14;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_2_channel_14_current_decode(statusFrame2.channel_14_current);
retData[*count].channel = 15;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_2_channel_15_current_decode(statusFrame2.channel_15_current);
retData[*count].channel = 16;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_2_channel_16_current_decode(statusFrame2.channel_16_current);
retData[*count].channel = 17;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_2_channel_17_current_decode(statusFrame2.channel_17_current);
retData[*count].channel = 18;
retData[*count].timestamp = timestamp;
(*count)++;
}
messagesRead = 0;
HAL_CAN_ReadStreamSession(hpdh->streamSessionHandles[3], messages, 50,
&messagesRead, status);
if (*status < 0) {
goto Exit;
}
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_3_t statusFrame3;
PDH_status_3_unpack(&statusFrame3, messages[i].data, PDH_STATUS_3_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
retData[*count].current =
PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current);
retData[*count].channel = 19;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_3_channel_19_current_decode(statusFrame3.channel_19_current);
retData[*count].channel = 20;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_3_channel_20_current_decode(statusFrame3.channel_20_current);
retData[*count].channel = 21;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_3_channel_21_current_decode(statusFrame3.channel_21_current);
retData[*count].channel = 22;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_3_channel_22_current_decode(statusFrame3.channel_22_current);
retData[*count].channel = 23;
retData[*count].timestamp = timestamp;
(*count)++;
retData[*count].current =
PDH_status_3_channel_23_current_decode(statusFrame3.channel_23_current);
retData[*count].channel = 24;
retData[*count].timestamp = timestamp;
(*count)++;
}
Exit:
if (*status < 0) {
delete[] retData;
retData = nullptr;
}
return retData;
}
void HAL_StopREVPDHStream(HAL_REVPDHHandle handle, int32_t* status) {
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (!hpdh->streamHandleAllocated) {
*status = RESOURCE_OUT_OF_RANGE;
return;
}
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[0]);
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[1]);
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[2]);
HAL_CAN_CloseStreamSession(hpdh->streamSessionHandles[3]);
hpdh->streamHandleAllocated = false;
}
} // extern "C"

170
hal/src/main/native/athena/REVPDH.h
View File

@@ -1,170 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdint.h>
#include "hal/PowerDistribution.h"
#include "hal/Types.h"
/**
* @defgroup hal_rev_pdh REV Power Distribution Hub API Functions
* @ingroup hal_capi
* @{
*/
#ifdef __cplusplus
extern "C" {
#endif
/**
* Initializes a REV Power Distribution Hub (PDH) device.
*
* @param module the device CAN ID (1 .. 63)
* @return the created PDH handle
*/
HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
const char* allocationLocation,
int32_t* status);
/**
* Frees a PDH device handle.
*
* @param handle the previously created PDH handle
*/
void HAL_FreeREVPDH(HAL_REVPDHHandle handle);
/**
* Gets the module number for a pdh.
*/
int32_t HAL_GetREVPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status);
/**
* Checks if a PDH module number is valid.
*
* Does not check if a PDH device with this module has been initialized.
*
* @param module module number (1 .. 63)
* @return 1 if the module number is valid; 0 otherwise
*/
HAL_Bool HAL_CheckREVPDHModuleNumber(int32_t module);
/**
* Checks if a PDH channel number is valid.
*
* @param module channel number (0 .. kNumREVPDHChannels)
* @return 1 if the channel number is valid; 0 otherwise
*/
HAL_Bool HAL_CheckREVPDHChannelNumber(int32_t channel);
/**
* Gets the current of a PDH channel in Amps.
*
* @param handle PDH handle
* @param channel the channel to retrieve the current of (0 ..
* kNumREVPDHChannels)
*
* @return the current of the PDH channel in Amps
*/
double HAL_GetREVPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel,
int32_t* status);
/**
* @param handle PDH handle
* @param currents array of currents
*/
void HAL_GetREVPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents,
int32_t* status);
/**
* Gets the total current of the PDH in Amps, measured to the nearest even
* integer.
*
* @param handle PDH handle
*
* @return the total current of the PDH in Amps
*/
uint16_t HAL_GetREVPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status);
/**
* Sets the state of the switchable channel on a PDH device.
*
* @param handle PDH handle
* @param enabled 1 if the switchable channel should be enabled; 0
* otherwise
*/
void HAL_SetREVPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled,
int32_t* status);
/**
* Gets the current state of the switchable channel on a PDH device.
*
* This call relies on a periodic status sent by the PDH device and will be as
* fresh as the last packet received.
*
* @param handle PDH handle
* @return 1 if the switchable channel is enabled; 0 otherwise
*/
HAL_Bool HAL_GetREVPDHSwitchableChannelState(HAL_REVPDHHandle handle,
int32_t* status);
/**
* Gets the firmware and hardware versions of a PDH device.
*
* @param handle PDH handle
*
* @return version information
*/
void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
HAL_PowerDistributionVersion* version,
int32_t* status);
/**
* Gets the voltage being supplied to a PDH device.
*
* @param handle PDH handle
*
* @return the voltage at the input of the PDH in Volts
*/
double HAL_GetREVPDHVoltage(HAL_REVPDHHandle handle, int32_t* status);
/**
* Gets the faults of a PDH device.
*
* @param handle PDH handle
*
* @return the faults of the PDH
*/
void HAL_GetREVPDHFaults(HAL_REVPDHHandle handle,
HAL_PowerDistributionFaults* faults, int32_t* status);
/**
* Gets the sticky faults of a PDH device.
*
* @param handle PDH handle
*
* @return the sticky faults of the PDH
*/
void HAL_GetREVPDHStickyFaults(HAL_REVPDHHandle handle,
HAL_PowerDistributionStickyFaults* stickyFaults,
int32_t* status);
/**
* Clears the sticky faults on a PDH device.
*
* @param handle PDH handle
*/
void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status);
void HAL_StartREVPDHStream(HAL_REVPDHHandle handle, int32_t* status);
HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
HAL_REVPDHHandle handle, int32_t* count, int32_t* status);
void HAL_StopREVPDHStream(HAL_REVPDHHandle handle, int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif

790
hal/src/main/native/athena/REVPH.cpp
View File

@@ -1,790 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/REVPH.h"
#include <string>
#include <thread>
#include <fmt/format.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/CANAPI.h"
#include "hal/Errors.h"
#include "hal/handles/IndexedHandleResource.h"
#include "rev/PHFrames.h"
using namespace hal;
static constexpr HAL_CANManufacturer manufacturer =
HAL_CANManufacturer::HAL_CAN_Man_kREV;
static constexpr HAL_CANDeviceType deviceType =
HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics;
static constexpr int32_t kDefaultControlPeriod = 20;
static constexpr uint8_t kDefaultCompressorDuty = 255;
static constexpr uint8_t kDefaultPressureTarget = 120;
static constexpr uint8_t kDefaultPressureHysteresis = 60;
#define HAL_REVPH_MAX_PULSE_TIME 65534
static constexpr uint32_t APIFromExtId(uint32_t extId) {
return (extId >> 6) & 0x3FF;
}
static constexpr uint32_t PH_STATUS_0_FRAME_API =
APIFromExtId(PH_STATUS_0_FRAME_ID);
static constexpr uint32_t PH_STATUS_1_FRAME_API =
APIFromExtId(PH_STATUS_1_FRAME_ID);
static constexpr uint32_t PH_SET_ALL_FRAME_API =
APIFromExtId(PH_SET_ALL_FRAME_ID);
static constexpr uint32_t PH_PULSE_ONCE_FRAME_API =
APIFromExtId(PH_PULSE_ONCE_FRAME_ID);
static constexpr uint32_t PH_COMPRESSOR_CONFIG_API =
APIFromExtId(PH_COMPRESSOR_CONFIG_FRAME_ID);
static constexpr uint32_t PH_CLEAR_FAULTS_FRAME_API =
APIFromExtId(PH_CLEAR_FAULTS_FRAME_ID);
static constexpr uint32_t PH_VERSION_FRAME_API =
APIFromExtId(PH_VERSION_FRAME_ID);
static constexpr int32_t kPHFrameStatus0Timeout = 50;
static constexpr int32_t kPHFrameStatus1Timeout = 50;
namespace {
struct REV_PHObj {
int32_t controlPeriod;
PH_set_all_t desiredSolenoidsState;
wpi::mutex solenoidLock;
HAL_CANHandle hcan;
std::string previousAllocation;
HAL_REVPHVersion versionInfo;
};
} // namespace
static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, 63,
HAL_HandleEnum::REVPH>* REVPHHandles;
namespace hal::init {
void InitializeREVPH() {
static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, kNumREVPHModules,
HAL_HandleEnum::REVPH>
rH;
REVPHHandles = &rH;
}
} // namespace hal::init
static PH_status_0_t HAL_ReadREVPHStatus0(HAL_CANHandle hcan, int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_status_0_t result = {};
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_0_FRAME_API, packedData, &length,
&timestamp, kPHFrameStatus0Timeout * 2, status);
if (*status != 0) {
return result;
}
PH_status_0_unpack(&result, packedData, PH_STATUS_0_LENGTH);
return result;
}
static PH_status_1_t HAL_ReadREVPHStatus1(HAL_CANHandle hcan, int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_status_1_t result = {};
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_1_FRAME_API, packedData, &length,
&timestamp, kPHFrameStatus1Timeout * 2, status);
if (*status != 0) {
return result;
}
PH_status_1_unpack(&result, packedData, PH_STATUS_1_LENGTH);
return result;
}
enum REV_SolenoidState {
kSolenoidDisabled = 0,
kSolenoidEnabled,
kSolenoidControlledViaPulse
};
static void HAL_UpdateDesiredREVPHSolenoidState(REV_PHObj* hph,
int32_t solenoid,
REV_SolenoidState state) {
switch (solenoid) {
case 0:
hph->desiredSolenoidsState.channel_0 = state;
break;
case 1:
hph->desiredSolenoidsState.channel_1 = state;
break;
case 2:
hph->desiredSolenoidsState.channel_2 = state;
break;
case 3:
hph->desiredSolenoidsState.channel_3 = state;
break;
case 4:
hph->desiredSolenoidsState.channel_4 = state;
break;
case 5:
hph->desiredSolenoidsState.channel_5 = state;
break;
case 6:
hph->desiredSolenoidsState.channel_6 = state;
break;
case 7:
hph->desiredSolenoidsState.channel_7 = state;
break;
case 8:
hph->desiredSolenoidsState.channel_8 = state;
break;
case 9:
hph->desiredSolenoidsState.channel_9 = state;
break;
case 10:
hph->desiredSolenoidsState.channel_10 = state;
break;
case 11:
hph->desiredSolenoidsState.channel_11 = state;
break;
case 12:
hph->desiredSolenoidsState.channel_12 = state;
break;
case 13:
hph->desiredSolenoidsState.channel_13 = state;
break;
case 14:
hph->desiredSolenoidsState.channel_14 = state;
break;
case 15:
hph->desiredSolenoidsState.channel_15 = state;
break;
}
}
static void HAL_SendREVPHSolenoidsState(REV_PHObj* hph, int32_t* status) {
uint8_t packedData[PH_SET_ALL_LENGTH] = {0};
PH_set_all_pack(packedData, &(hph->desiredSolenoidsState), PH_SET_ALL_LENGTH);
HAL_WriteCANPacketRepeating(hph->hcan, packedData, PH_SET_ALL_LENGTH,
PH_SET_ALL_FRAME_API, hph->controlPeriod, status);
}
static HAL_Bool HAL_CheckREVPHPulseTime(int32_t time) {
return ((time > 0) && (time <= HAL_REVPH_MAX_PULSE_TIME)) ? 1 : 0;
}
HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
if (!HAL_CheckREVPHModuleNumber(module)) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
kNumREVPHModules, module);
return HAL_kInvalidHandle;
}
HAL_REVPHHandle handle;
// Module starts at 1
auto hph = REVPHHandles->Allocate(module - 1, &handle, status);
if (*status != 0) {
if (hph) {
hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module,
hph->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
kNumREVPHModules, module);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
HAL_CANHandle hcan =
HAL_InitializeCAN(manufacturer, module, deviceType, status);
if (*status != 0) {
REVPHHandles->Free(handle);
return HAL_kInvalidHandle;
}
hph->previousAllocation = allocationLocation ? allocationLocation : "";
hph->hcan = hcan;
hph->controlPeriod = kDefaultControlPeriod;
std::memset(&hph->desiredSolenoidsState, 0,
sizeof(hph->desiredSolenoidsState));
std::memset(&hph->versionInfo, 0, sizeof(hph->versionInfo));
int32_t can_status = 0;
// Start closed-loop compressor control by starting solenoid state updates
HAL_SendREVPHSolenoidsState(hph.get(), &can_status);
return handle;
}
void HAL_FreeREVPH(HAL_REVPHHandle handle) {
auto hph = REVPHHandles->Get(handle);
if (hph) {
HAL_CleanCAN(hph->hcan);
}
REVPHHandles->Free(handle);
}
HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) {
return module >= 1 && module <= kNumREVPHModules;
}
HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) {
return channel >= 0 && channel < kNumREVPHChannels;
}
HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return false;
}
return status0.compressor_on;
}
void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle,
const HAL_REVPHCompressorConfig* config,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PH_compressor_config_t frameData;
frameData.minimum_tank_pressure =
PH_compressor_config_minimum_tank_pressure_encode(
config->minAnalogVoltage);
frameData.maximum_tank_pressure =
PH_compressor_config_maximum_tank_pressure_encode(
config->maxAnalogVoltage);
frameData.force_disable = config->forceDisable;
frameData.use_digital = config->useDigital;
uint8_t packedData[PH_COMPRESSOR_CONFIG_LENGTH] = {0};
PH_compressor_config_pack(packedData, &frameData,
PH_COMPRESSOR_CONFIG_LENGTH);
HAL_WriteCANPacket(ph->hcan, packedData, PH_COMPRESSOR_CONFIG_LENGTH,
PH_COMPRESSOR_CONFIG_API, status);
}
void HAL_SetREVPHClosedLoopControlDisabled(HAL_REVPHHandle handle,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.forceDisable = true;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
void HAL_SetREVPHClosedLoopControlDigital(HAL_REVPHHandle handle,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.useDigital = true;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
void HAL_SetREVPHClosedLoopControlAnalog(HAL_REVPHHandle handle,
double minAnalogVoltage,
double maxAnalogVoltage,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.minAnalogVoltage = minAnalogVoltage;
config.maxAnalogVoltage = maxAnalogVoltage;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
void HAL_SetREVPHClosedLoopControlHybrid(HAL_REVPHHandle handle,
double minAnalogVoltage,
double maxAnalogVoltage,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.minAnalogVoltage = minAnalogVoltage;
config.maxAnalogVoltage = maxAnalogVoltage;
config.useDigital = true;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
HAL_REVPHCompressorConfigType HAL_GetREVPHCompressorConfig(
HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return HAL_REVPHCompressorConfigType_kDisabled;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return HAL_REVPHCompressorConfigType_kDisabled;
}
return static_cast<HAL_REVPHCompressorConfigType>(status0.compressor_config);
}
HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return false;
}
return status0.digital_sensor;
}
double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_compressor_current_decode(status1.compressor_current);
}
double HAL_GetREVPHAnalogVoltage(HAL_REVPHHandle handle, int32_t channel,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (channel < 0 || channel > 1) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid REV Analog Index", 0, 2,
channel);
return 0;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return 0;
}
if (channel == 0) {
return PH_status_0_analog_0_decode(status0.analog_0);
}
return PH_status_0_analog_1_decode(status0.analog_1);
}
double HAL_GetREVPHVoltage(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_v_bus_decode(status1.v_bus);
}
double HAL_GetREVPH5VVoltage(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_supply_voltage_5_v_decode(status1.supply_voltage_5_v);
}
double HAL_GetREVPHSolenoidCurrent(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_solenoid_current_decode(status1.solenoid_current);
}
double HAL_GetREVPHSolenoidVoltage(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_solenoid_voltage_decode(status1.solenoid_voltage);
}
void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
int32_t* status) {
std::memset(version, 0, sizeof(*version));
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_version_t result = {};
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (ph->versionInfo.firmwareMajor > 0) {
version->firmwareMajor = ph->versionInfo.firmwareMajor;
version->firmwareMinor = ph->versionInfo.firmwareMinor;
version->firmwareFix = ph->versionInfo.firmwareFix;
version->hardwareMajor = ph->versionInfo.hardwareMajor;
version->hardwareMinor = ph->versionInfo.hardwareMinor;
version->uniqueId = ph->versionInfo.uniqueId;
*status = 0;
return;
}
HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API,
status);
if (*status != 0) {
return;
}
uint32_t timeoutMs = 100;
for (uint32_t i = 0; i <= timeoutMs; i++) {
HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, packedData, &length,
&timestamp, status);
if (*status == 0) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
if (*status != 0) {
return;
}
PH_version_unpack(&result, packedData, PH_VERSION_LENGTH);
version->firmwareMajor = result.firmware_year;
version->firmwareMinor = result.firmware_minor;
version->firmwareFix = result.firmware_fix;
version->hardwareMinor = result.hardware_minor;
version->hardwareMajor = result.hardware_major;
version->uniqueId = result.unique_id;
ph->versionInfo = *version;
}
int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return 0;
}
uint32_t result = status0.channel_0;
result |= status0.channel_1 << 1;
result |= status0.channel_2 << 2;
result |= status0.channel_3 << 3;
result |= status0.channel_4 << 4;
result |= status0.channel_5 << 5;
result |= status0.channel_6 << 6;
result |= status0.channel_7 << 7;
result |= status0.channel_8 << 8;
result |= status0.channel_9 << 9;
result |= status0.channel_10 << 10;
result |= status0.channel_11 << 11;
result |= status0.channel_12 << 12;
result |= status0.channel_13 << 13;
result |= status0.channel_14 << 14;
result |= status0.channel_15 << 15;
return result;
}
void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
std::scoped_lock lock{ph->solenoidLock};
for (int solenoid = 0; solenoid < kNumREVPHChannels; solenoid++) {
if (mask & (1 << solenoid)) {
// The mask bit for the solenoid is set, so we update the solenoid state
REV_SolenoidState desiredSolenoidState =
values & (1 << solenoid) ? kSolenoidEnabled : kSolenoidDisabled;
HAL_UpdateDesiredREVPHSolenoidState(ph.get(), solenoid,
desiredSolenoidState);
}
}
HAL_SendREVPHSolenoidsState(ph.get(), status);
}
void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (index >= kNumREVPHChannels || index < 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Only [0-15] are valid index values. Requested {}", index));
return;
}
if (!HAL_CheckREVPHPulseTime(durMs)) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Time not within expected range [0-65534]. Requested {}",
durMs));
return;
}
{
std::scoped_lock lock{ph->solenoidLock};
HAL_UpdateDesiredREVPHSolenoidState(ph.get(), index,
kSolenoidControlledViaPulse);
HAL_SendREVPHSolenoidsState(ph.get(), status);
}
if (*status != 0) {
return;
}
PH_pulse_once_t pulse = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
pulse.pulse_length_ms = durMs;
// Specify which solenoid should be pulsed
// The protocol supports specifying any number of solenoids to be pulsed at
// the same time, should that functionality be exposed to users in the future.
switch (index) {
case 0:
pulse.channel_0 = true;
break;
case 1:
pulse.channel_1 = true;
break;
case 2:
pulse.channel_2 = true;
break;
case 3:
pulse.channel_3 = true;
break;
case 4:
pulse.channel_4 = true;
break;
case 5:
pulse.channel_5 = true;
break;
case 6:
pulse.channel_6 = true;
break;
case 7:
pulse.channel_7 = true;
break;
case 8:
pulse.channel_8 = true;
break;
case 9:
pulse.channel_9 = true;
break;
case 10:
pulse.channel_10 = true;
break;
case 11:
pulse.channel_11 = true;
break;
case 12:
pulse.channel_12 = true;
break;
case 13:
pulse.channel_13 = true;
break;
case 14:
pulse.channel_14 = true;
break;
case 15:
pulse.channel_15 = true;
break;
}
// Send pulse command
uint8_t packedData[PH_PULSE_ONCE_LENGTH] = {0};
PH_pulse_once_pack(packedData, &pulse, PH_PULSE_ONCE_LENGTH);
HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH,
PH_PULSE_ONCE_FRAME_API, status);
}
void HAL_GetREVPHFaults(HAL_REVPHHandle handle, HAL_REVPHFaults* faults,
int32_t* status) {
std::memset(faults, 0, sizeof(*faults));
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
faults->channel0Fault = status0.channel_0_fault;
faults->channel1Fault = status0.channel_1_fault;
faults->channel2Fault = status0.channel_2_fault;
faults->channel3Fault = status0.channel_3_fault;
faults->channel4Fault = status0.channel_4_fault;
faults->channel5Fault = status0.channel_5_fault;
faults->channel6Fault = status0.channel_6_fault;
faults->channel7Fault = status0.channel_7_fault;
faults->channel8Fault = status0.channel_8_fault;
faults->channel9Fault = status0.channel_9_fault;
faults->channel10Fault = status0.channel_10_fault;
faults->channel11Fault = status0.channel_11_fault;
faults->channel12Fault = status0.channel_12_fault;
faults->channel13Fault = status0.channel_13_fault;
faults->channel14Fault = status0.channel_14_fault;
faults->channel15Fault = status0.channel_15_fault;
faults->compressorOverCurrent = status0.compressor_oc_fault;
faults->compressorOpen = status0.compressor_open_fault;
faults->solenoidOverCurrent = status0.solenoid_oc_fault;
faults->brownout = status0.brownout_fault;
faults->canWarning = status0.can_warning_fault;
faults->hardwareFault = status0.hardware_fault;
}
void HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle,
HAL_REVPHStickyFaults* stickyFaults,
int32_t* status) {
std::memset(stickyFaults, 0, sizeof(*stickyFaults));
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
stickyFaults->compressorOverCurrent = status1.sticky_compressor_oc_fault;
stickyFaults->compressorOpen = status1.sticky_compressor_open_fault;
stickyFaults->solenoidOverCurrent = status1.sticky_solenoid_oc_fault;
stickyFaults->brownout = status1.sticky_brownout_fault;
stickyFaults->canWarning = status1.sticky_can_warning_fault;
stickyFaults->canBusOff = status1.sticky_can_bus_off_fault;
stickyFaults->hardwareFault = status1.sticky_hardware_fault;
stickyFaults->firmwareFault = status1.sticky_firmware_fault;
stickyFaults->hasReset = status1.sticky_has_reset_fault;
}
void HAL_ClearREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t packedData[8] = {0};
HAL_WriteCANPacket(ph->hcan, packedData, PH_CLEAR_FAULTS_LENGTH,
PH_CLEAR_FAULTS_FRAME_API, status);
}
int32_t HAL_GetREVPHSolenoidDisabledList(HAL_REVPHHandle handle,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return 0;
}
uint32_t solenoidFaults = status0.channel_0_fault;
solenoidFaults |= status0.channel_1_fault << 1;
solenoidFaults |= status0.channel_2_fault << 2;
solenoidFaults |= status0.channel_3_fault << 3;
solenoidFaults |= status0.channel_4_fault << 4;
solenoidFaults |= status0.channel_5_fault << 5;
solenoidFaults |= status0.channel_6_fault << 6;
solenoidFaults |= status0.channel_7_fault << 7;
solenoidFaults |= status0.channel_8_fault << 8;
solenoidFaults |= status0.channel_9_fault << 9;
solenoidFaults |= status0.channel_10_fault << 10;
solenoidFaults |= status0.channel_11_fault << 11;
solenoidFaults |= status0.channel_12_fault << 12;
solenoidFaults |= status0.channel_13_fault << 13;
solenoidFaults |= status0.channel_14_fault << 14;
solenoidFaults |= status0.channel_15_fault << 15;
return solenoidFaults;
}

155
hal/src/main/native/athena/Relay.cpp
View File

@@ -1,155 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Relay.h"
#include <string>
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/handles/IndexedHandleResource.h"
using namespace hal;
namespace {
struct Relay {
uint8_t channel;
bool fwd;
std::string previousAllocation;
};
} // namespace
static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
HAL_HandleEnum::Relay>* relayHandles;
// Create a mutex to protect changes to the relay values
static wpi::mutex digitalRelayMutex;
namespace hal::init {
void InitializeRelay() {
static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
HAL_HandleEnum::Relay>
rH;
relayHandles = &rH;
}
} // namespace hal::init
extern "C" {
HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
initializeDigital(status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
int16_t channel = getPortHandleChannel(portHandle);
if (channel == InvalidHandleIndex || channel >= kNumRelayChannels) {
*status = RESOURCE_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
kNumRelayChannels, channel);
return HAL_kInvalidHandle;
}
if (!fwd) {
channel += kNumRelayHeaders; // add 4 to reverse channels
}
HAL_RelayHandle handle;
auto port = relayHandles->Allocate(channel, &handle, status);
if (*status != 0) {
if (port) {
hal::SetLastErrorPreviouslyAllocated(status, "Relay", channel,
port->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
kNumRelayChannels, channel);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
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);
port->previousAllocation = allocationLocation ? allocationLocation : "";
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
// separate functions for forward and reverse,
// instead of separate channel IDs
return channel < kNumRelayHeaders && channel >= 0;
}
void HAL_SetRelay(HAL_RelayHandle relayPortHandle, HAL_Bool on,
int32_t* status) {
auto port = relayHandles->Get(relayPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
std::scoped_lock lock(digitalRelayMutex);
uint8_t relays = 0;
if (port->fwd) {
relays = relaySystem->readValue_Forward(status);
} else {
relays = relaySystem->readValue_Reverse(status);
}
if (*status != 0) {
return; // bad status read
}
if (on) {
relays |= 1 << port->channel;
} else {
relays &= ~(1 << port->channel);
}
if (port->fwd) {
relaySystem->writeValue_Forward(relays, status);
} else {
relaySystem->writeValue_Reverse(relays, status);
}
}
HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status) {
auto port = relayHandles->Get(relayPortHandle);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
uint8_t relays = 0;
if (port->fwd) {
relays = relaySystem->readValue_Forward(status);
} else {
relays = relaySystem->readValue_Reverse(status);
}
return (relays & (1 << port->channel)) != 0;
}
} // extern "C"

714
hal/src/main/native/athena/SPI.cpp
View File

@@ -1,714 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/SPI.h"
#include <fcntl.h>
#include <linux/spi/spidev.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <array>
#include <atomic>
#include <cstdio>
#include <cstring>
#include <memory>
#include <fmt/format.h>
#include <wpi/mutex.h>
#include <wpi/print.h>
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "HALInternal.h"
#include "hal/DIO.h"
#include "hal/HAL.h"
#include "hal/handles/HandlesInternal.h"
using namespace hal;
static int32_t m_spiCS0Handle{0};
static int32_t m_spiCS1Handle{0};
static int32_t m_spiCS2Handle{0};
static int32_t m_spiCS3Handle{0};
static int32_t m_spiMXPHandle{0};
static constexpr int32_t kSpiMaxHandles = 5;
// Indices 0-3 are for onboard CS0-CS2. Index 4 is for MXP.
static std::array<wpi::mutex, kSpiMaxHandles> spiHandleMutexes;
static std::array<wpi::mutex, kSpiMaxHandles> spiApiMutexes;
static std::array<wpi::mutex, kSpiMaxHandles> spiAccumulatorMutexes;
// MXP SPI does not count towards this
static std::atomic<int32_t> spiPortCount{0};
static HAL_DigitalHandle digitalHandles[9]{HAL_kInvalidHandle};
static wpi::mutex spiAutoMutex;
static int32_t spiAutoPort = kSpiMaxHandles;
static std::atomic_bool spiAutoRunning{false};
static std::unique_ptr<tDMAManager> spiAutoDMA;
static bool SPIInUseByAuto(HAL_SPIPort port) {
// SPI engine conflicts with any other chip selects on the same SPI device.
// There are two SPI devices: one for ports 0-3 (onboard), the other for port
// 4 (MXP).
if (!spiAutoRunning) {
return false;
}
std::scoped_lock lock(spiAutoMutex);
return (spiAutoPort >= 0 && spiAutoPort <= 3 && port >= 0 && port <= 3) ||
(spiAutoPort == 4 && port == 4);
}
namespace hal::init {
void InitializeSPI() {}
} // namespace hal::init
extern "C" {
static void CommonSPIPortInit(int32_t* status) {
// All false cases will set
if (spiPortCount.fetch_add(1) == 0) {
// Have not been initialized yet
initializeDigital(status);
if (*status != 0) {
return;
}
// MISO
if ((digitalHandles[3] = HAL_InitializeDIOPort(createPortHandleForSPI(29),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 29 (MISO)");
return;
}
// MOSI
if ((digitalHandles[4] = HAL_InitializeDIOPort(createPortHandleForSPI(30),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 30 (MOSI)");
HAL_FreeDIOPort(digitalHandles[3]); // free the first port allocated
return;
}
}
}
static void CommonSPIPortFree(void) {
if (spiPortCount.fetch_sub(1) == 1) {
// Clean up SPI Handles
HAL_FreeDIOPort(digitalHandles[3]);
HAL_FreeDIOPort(digitalHandles[4]);
}
}
void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
hal::init::CheckInit();
if (port < 0 || port >= kSpiMaxHandles) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Serial port must be between 0 and {}. Requested {}",
kSpiMaxHandles, static_cast<int>(port)));
return;
}
int handle;
if (HAL_GetSPIHandle(port) != 0) {
return;
}
switch (port) {
case HAL_SPI_kOnboardCS0:
CommonSPIPortInit(status);
if (*status != 0) {
return;
}
// CS0 is not a DIO port, so nothing to allocate
handle = open("/dev/spidev0.0", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open SPI port {}: {}\n",
static_cast<int32_t>(port), std::strerror(errno));
CommonSPIPortFree();
return;
}
HAL_SetSPIHandle(HAL_SPI_kOnboardCS0, handle);
break;
case HAL_SPI_kOnboardCS1:
CommonSPIPortInit(status);
if (*status != 0) {
return;
}
// CS1, Allocate
if ((digitalHandles[0] = HAL_InitializeDIOPort(createPortHandleForSPI(26),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 26 (CS1)");
CommonSPIPortFree();
return;
}
handle = open("/dev/spidev0.1", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open SPI port {}: {}\n",
static_cast<int32_t>(port), std::strerror(errno));
CommonSPIPortFree();
HAL_FreeDIOPort(digitalHandles[0]);
return;
}
HAL_SetSPIHandle(HAL_SPI_kOnboardCS1, handle);
break;
case HAL_SPI_kOnboardCS2:
CommonSPIPortInit(status);
if (*status != 0) {
return;
}
// CS2, Allocate
if ((digitalHandles[1] = HAL_InitializeDIOPort(createPortHandleForSPI(27),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 27 (CS2)");
CommonSPIPortFree();
return;
}
handle = open("/dev/spidev0.2", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open SPI port {}: {}\n",
static_cast<int32_t>(port), std::strerror(errno));
CommonSPIPortFree();
HAL_FreeDIOPort(digitalHandles[1]);
return;
}
HAL_SetSPIHandle(HAL_SPI_kOnboardCS2, handle);
break;
case HAL_SPI_kOnboardCS3:
CommonSPIPortInit(status);
if (*status != 0) {
return;
}
// CS3, Allocate
if ((digitalHandles[2] = HAL_InitializeDIOPort(createPortHandleForSPI(28),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 28 (CS3)");
CommonSPIPortFree();
return;
}
handle = open("/dev/spidev0.3", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open SPI port {}: {}\n",
static_cast<int32_t>(port), std::strerror(errno));
CommonSPIPortFree();
HAL_FreeDIOPort(digitalHandles[2]);
return;
}
HAL_SetSPIHandle(HAL_SPI_kOnboardCS3, handle);
break;
case HAL_SPI_kMXP:
initializeDigital(status);
if (*status != 0) {
return;
}
if ((digitalHandles[5] = HAL_InitializeDIOPort(createPortHandleForSPI(14),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 14");
return;
}
if ((digitalHandles[6] = HAL_InitializeDIOPort(createPortHandleForSPI(15),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 15");
HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
return;
}
if ((digitalHandles[7] = HAL_InitializeDIOPort(createPortHandleForSPI(16),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 16");
HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated
return;
}
if ((digitalHandles[8] = HAL_InitializeDIOPort(createPortHandleForSPI(17),
false, nullptr, status)) ==
HAL_kInvalidHandle) {
std::puts("Failed to allocate DIO 17");
HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated
HAL_FreeDIOPort(digitalHandles[7]); // free the third port allocated
return;
}
digitalSystem->writeEnableMXPSpecialFunction(
digitalSystem->readEnableMXPSpecialFunction(status) | 0x00F0, status);
handle = open("/dev/spidev1.0", O_RDWR);
if (handle < 0) {
wpi::print("Failed to open SPI port {}: {}\n",
static_cast<int32_t>(port), std::strerror(errno));
HAL_FreeDIOPort(digitalHandles[5]); // free the first port allocated
HAL_FreeDIOPort(digitalHandles[6]); // free the second port allocated
HAL_FreeDIOPort(digitalHandles[7]); // free the third port allocated
HAL_FreeDIOPort(digitalHandles[8]); // free the fourth port allocated
return;
}
HAL_SetSPIHandle(HAL_SPI_kMXP, handle);
break;
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status, fmt::format("Invalid SPI port {}", static_cast<int>(port)));
break;
}
}
int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
uint8_t* dataReceived, int32_t size) {
if (port < 0 || port >= kSpiMaxHandles) {
return -1;
}
if (SPIInUseByAuto(port)) {
return -1;
}
struct spi_ioc_transfer xfer;
std::memset(&xfer, 0, sizeof(xfer));
xfer.tx_buf = (__u64)dataToSend;
xfer.rx_buf = (__u64)dataReceived;
xfer.len = size;
std::scoped_lock lock(spiApiMutexes[port]);
return ioctl(HAL_GetSPIHandle(port), SPI_IOC_MESSAGE(1), &xfer);
}
int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t sendSize) {
if (port < 0 || port >= kSpiMaxHandles) {
return -1;
}
if (SPIInUseByAuto(port)) {
return -1;
}
struct spi_ioc_transfer xfer;
std::memset(&xfer, 0, sizeof(xfer));
xfer.tx_buf = (__u64)dataToSend;
xfer.len = sendSize;
std::scoped_lock lock(spiApiMutexes[port]);
return ioctl(HAL_GetSPIHandle(port), SPI_IOC_MESSAGE(1), &xfer);
}
int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) {
if (port < 0 || port >= kSpiMaxHandles) {
return -1;
}
if (SPIInUseByAuto(port)) {
return -1;
}
struct spi_ioc_transfer xfer;
std::memset(&xfer, 0, sizeof(xfer));
xfer.rx_buf = (__u64)buffer;
xfer.len = count;
std::scoped_lock lock(spiApiMutexes[port]);
return ioctl(HAL_GetSPIHandle(port), SPI_IOC_MESSAGE(1), &xfer);
}
void HAL_CloseSPI(HAL_SPIPort port) {
if (port < 0 || port >= kSpiMaxHandles) {
return;
}
int32_t status = 0;
HAL_FreeSPIAuto(port, &status);
{
std::scoped_lock lock(spiApiMutexes[port]);
close(HAL_GetSPIHandle(port));
}
HAL_SetSPIHandle(port, 0);
if (port < 4) {
CommonSPIPortFree();
}
switch (port) {
// Case 0 does not need to do anything
case 1:
HAL_FreeDIOPort(digitalHandles[0]);
break;
case 2:
HAL_FreeDIOPort(digitalHandles[1]);
break;
case 3:
HAL_FreeDIOPort(digitalHandles[2]);
break;
case 4:
HAL_FreeDIOPort(digitalHandles[5]);
HAL_FreeDIOPort(digitalHandles[6]);
HAL_FreeDIOPort(digitalHandles[7]);
HAL_FreeDIOPort(digitalHandles[8]);
break;
default:
break;
}
}
void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {
if (port < 0 || port >= kSpiMaxHandles) {
return;
}
std::scoped_lock lock(spiApiMutexes[port]);
ioctl(HAL_GetSPIHandle(port), SPI_IOC_WR_MAX_SPEED_HZ, &speed);
}
void HAL_SetSPIMode(HAL_SPIPort port, HAL_SPIMode mode) {
if (port < 0 || port >= kSpiMaxHandles) {
return;
}
uint8_t mode8 = mode & SPI_MODE_3;
std::scoped_lock lock(spiApiMutexes[port]);
ioctl(HAL_GetSPIHandle(port), SPI_IOC_WR_MODE, &mode8);
}
HAL_SPIMode HAL_GetSPIMode(HAL_SPIPort port) {
if (port < 0 || port >= kSpiMaxHandles) {
return HAL_SPI_kMode0;
}
uint8_t mode8 = 0;
std::scoped_lock lock(spiApiMutexes[port]);
ioctl(HAL_GetSPIHandle(port), SPI_IOC_RD_MODE, &mode8);
return static_cast<HAL_SPIMode>(mode8 & SPI_MODE_3);
}
void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {
if (port < 0 || port >= kSpiMaxHandles) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Serial port must be between 0 and {}. Requested {}",
kSpiMaxHandles, static_cast<int>(port)));
return;
}
std::scoped_lock lock(spiApiMutexes[port]);
if (port < 4) {
spiSystem->writeChipSelectActiveHigh_Hdr(
spiSystem->readChipSelectActiveHigh_Hdr(status) | (1 << port), status);
} else {
spiSystem->writeChipSelectActiveHigh_MXP(1, status);
}
}
void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {
if (port < 0 || port >= kSpiMaxHandles) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Serial port must be between 0 and {}. Requested {}",
kSpiMaxHandles, static_cast<int>(port)));
return;
}
std::scoped_lock lock(spiApiMutexes[port]);
if (port < 4) {
spiSystem->writeChipSelectActiveHigh_Hdr(
spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1 << port), status);
} else {
spiSystem->writeChipSelectActiveHigh_MXP(0, status);
}
}
int32_t HAL_GetSPIHandle(HAL_SPIPort port) {
if (port < 0 || port >= kSpiMaxHandles) {
return 0;
}
std::scoped_lock lock(spiHandleMutexes[port]);
switch (port) {
case 0:
return m_spiCS0Handle;
case 1:
return m_spiCS1Handle;
case 2:
return m_spiCS2Handle;
case 3:
return m_spiCS3Handle;
case 4:
return m_spiMXPHandle;
default:
return 0;
}
}
void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {
if (port < 0 || port >= kSpiMaxHandles) {
return;
}
std::scoped_lock lock(spiHandleMutexes[port]);
switch (port) {
case 0:
m_spiCS0Handle = handle;
break;
case 1:
m_spiCS1Handle = handle;
break;
case 2:
m_spiCS2Handle = handle;
break;
case 3:
m_spiCS3Handle = handle;
break;
case 4:
m_spiMXPHandle = handle;
break;
default:
break;
}
}
void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {
if (port < 0 || port >= kSpiMaxHandles) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Serial port must be between 0 and {}. Requested {}",
kSpiMaxHandles, static_cast<int>(port)));
return;
}
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (spiAutoPort != kSpiMaxHandles) {
*status = RESOURCE_IS_ALLOCATED;
return;
}
// remember the initialized port for other entry points
spiAutoPort = port;
// configure the correct chip select
if (port < 4) {
spiSystem->writeAutoSPI1Select(false, status);
spiSystem->writeAutoChipSelect(port, status);
} else {
spiSystem->writeAutoSPI1Select(true, status);
spiSystem->writeAutoChipSelect(0, status);
}
// configure DMA
spiAutoDMA =
std::make_unique<tDMAManager>(g_SpiAutoData_index, bufferSize, status);
}
void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {
if (port < 0 || port >= kSpiMaxHandles) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Serial port must be between 0 and {}. Requested {}",
kSpiMaxHandles, static_cast<int>(port)));
return;
}
std::scoped_lock lock(spiAutoMutex);
if (spiAutoPort != port) {
return;
}
spiAutoPort = kSpiMaxHandles;
// disable by setting to internal clock and setting rate=0
spiSystem->writeAutoRate(0, status);
spiSystem->writeAutoTriggerConfig_ExternalClock(false, status);
// stop the DMA
spiAutoDMA->stop(status);
spiAutoDMA.reset(nullptr);
spiAutoRunning = false;
}
void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
spiAutoRunning = true;
// start the DMA
spiAutoDMA->start(status);
// auto rate is in microseconds
spiSystem->writeAutoRate(period * 1000000, status);
// disable the external clock
spiSystem->writeAutoTriggerConfig_ExternalClock(false, status);
}
void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_Bool triggerRising, HAL_Bool triggerFalling,
int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
spiAutoRunning = true;
// start the DMA
spiAutoDMA->start(status);
// get channel routing
bool routingAnalogTrigger = false;
uint8_t routingChannel = 0;
uint8_t routingModule = 0;
if (!remapDigitalSource(digitalSourceHandle, analogTriggerType,
routingChannel, routingModule,
routingAnalogTrigger)) {
*status = HAL_HANDLE_ERROR;
return;
}
// configure external trigger and enable it
tSPI::tAutoTriggerConfig config;
config.ExternalClock = 1;
config.FallingEdge = triggerFalling ? 1 : 0;
config.RisingEdge = triggerRising ? 1 : 0;
config.ExternalClockSource_AnalogTrigger = routingAnalogTrigger ? 1 : 0;
config.ExternalClockSource_Module = routingModule;
config.ExternalClockSource_Channel = routingChannel;
spiSystem->writeAutoTriggerConfig(config, status);
}
void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
// disable by setting to internal clock and setting rate=0
spiSystem->writeAutoRate(0, status);
spiSystem->writeAutoTriggerConfig_ExternalClock(false, status);
// stop the DMA
spiAutoDMA->stop(status);
spiAutoRunning = false;
}
void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t dataSize, int32_t zeroSize,
int32_t* status) {
if (dataSize < 0 || dataSize > 32) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format(
"Data size must be between 0 and 32 inclusive. Requested {}",
dataSize));
return;
}
if (zeroSize < 0 || zeroSize > 127) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format(
"Zero size must be between 0 and 127 inclusive. Requested {}",
zeroSize));
return;
}
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
// set tx data registers
for (int32_t i = 0; i < dataSize; ++i) {
spiSystem->writeAutoTx(i >> 2, i & 3, dataToSend[i], status);
}
// set byte counts
tSPI::tAutoByteCount config;
config.ZeroByteCount = static_cast<unsigned>(zeroSize) & 0x7f;
config.TxByteCount = static_cast<unsigned>(dataSize) & 0x1f;
spiSystem->writeAutoByteCount(config, status);
}
void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
spiSystem->strobeAutoForceOne(status);
}
int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
int32_t numToRead, double timeout,
int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return 0;
}
size_t numRemaining = 0;
// timeout is in ms
spiAutoDMA->read(buffer, numToRead, timeout * 1000, &numRemaining, status);
return numRemaining;
}
int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return 0;
}
return spiSystem->readTransferSkippedFullCount(status);
}
void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
int32_t stallTicks, int32_t pow2BytesPerRead,
int32_t* status) {
std::scoped_lock lock(spiAutoMutex);
// FPGA only has one auto SPI engine
if (port != spiAutoPort) {
*status = INCOMPATIBLE_STATE;
return;
}
tSPI::tStallConfig stallConfig;
stallConfig.CsToSclkTicks = static_cast<uint8_t>(csToSclkTicks);
stallConfig.StallTicks = static_cast<uint16_t>(stallTicks);
stallConfig.Pow2BytesPerRead = static_cast<uint8_t>(pow2BytesPerRead);
spiSystem->writeStallConfig(stallConfig, status);
}
} // extern "C"

518
hal/src/main/native/athena/SerialPort.cpp
View File

@@ -1,518 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/SerialPort.h"
#include <fcntl.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <unistd.h>
#include <cerrno>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <stdexcept>
#include <string>
#include <thread>
#include <fmt/format.h>
#include "HALInternal.h"
#include "hal/cpp/SerialHelper.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/IndexedHandleResource.h"
namespace {
struct SerialPort {
int portId;
struct termios tty;
int baudRate;
double timeout = 0;
bool termination = false;
char terminationChar = '\n';
};
} // namespace
namespace hal {
IndexedHandleResource<HAL_SerialPortHandle, SerialPort, 4,
HAL_HandleEnum::SerialPort>* serialPortHandles;
} // namespace hal
namespace hal::init {
void InitializeSerialPort() {
static IndexedHandleResource<HAL_SerialPortHandle, SerialPort, 4,
HAL_HandleEnum::SerialPort>
spH;
serialPortHandles = &spH;
}
} // namespace hal::init
using namespace hal;
extern "C" {
HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port,
int32_t* status) {
// hal::init::CheckInit();
hal::SerialHelper serialHelper;
std::string portName = serialHelper.GetOSSerialPortName(port, status);
if (*status < 0) {
return HAL_kInvalidHandle;
}
return HAL_InitializeSerialPortDirect(port, portName.c_str(), status);
}
HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port,
const char* portName,
int32_t* status) {
HAL_SerialPortHandle handle;
auto serialPort =
serialPortHandles->Allocate(static_cast<int16_t>(port), &handle, status);
if (*status != 0) {
return HAL_kInvalidHandle;
}
serialPort->portId = open(portName, O_RDWR | O_NOCTTY);
if (serialPort->portId < 0) {
*status = -errno;
if (*status == EACCES) {
*status = HAL_CONSOLE_OUT_ENABLED_ERROR;
}
serialPortHandles->Free(handle);
return HAL_kInvalidHandle;
}
std::memset(&serialPort->tty, 0, sizeof(serialPort->tty));
serialPort->baudRate = B9600;
if (cfsetospeed(&serialPort->tty,
static_cast<speed_t>(serialPort->baudRate)) != 0) {
*status = -errno;
close(serialPort->portId);
serialPortHandles->Free(handle);
return HAL_kInvalidHandle;
}
if (cfsetispeed(&serialPort->tty,
static_cast<speed_t>(serialPort->baudRate)) != 0) {
*status = -errno;
close(serialPort->portId);
serialPortHandles->Free(handle);
return HAL_kInvalidHandle;
}
serialPort->tty.c_cflag &= ~PARENB;
serialPort->tty.c_cflag &= ~CSTOPB;
serialPort->tty.c_cflag &= ~CSIZE;
serialPort->tty.c_cflag |= CS8;
serialPort->tty.c_cc[VMIN] = 0;
serialPort->tty.c_cc[VTIME] = 10;
serialPort->tty.c_cflag |= CREAD | CLOCAL;
serialPort->tty.c_lflag &= ~(ICANON | ECHO | ISIG);
serialPort->tty.c_iflag &= ~(IXON | IXOFF | IXANY);
/* Raw output mode, sends the raw and unprocessed data (send as it is).
* If it is in canonical mode and sending new line char then CR
* will be added as prefix and send as CR LF
*/
serialPort->tty.c_oflag = ~OPOST;
if (tcflush(serialPort->portId, TCIOFLUSH) != 0) {
*status = -errno;
close(serialPort->portId);
serialPortHandles->Free(handle);
return HAL_kInvalidHandle;
}
if (tcsetattr(serialPort->portId, TCSANOW, &serialPort->tty) != 0) {
*status = -errno;
close(serialPort->portId);
serialPortHandles->Free(handle);
return HAL_kInvalidHandle;
}
return handle;
}
void HAL_CloseSerial(HAL_SerialPortHandle handle) {
auto port = serialPortHandles->Get(handle);
serialPortHandles->Free(handle);
if (port) {
close(port->portId);
}
}
int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return -1;
}
return port->portId;
}
#define BAUDCASE(BAUD) \
case BAUD: \
port->baudRate = B##BAUD; \
break;
void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
switch (baud) {
BAUDCASE(50)
BAUDCASE(75)
BAUDCASE(110)
BAUDCASE(134)
BAUDCASE(150)
BAUDCASE(200)
BAUDCASE(300)
BAUDCASE(600)
BAUDCASE(1200)
BAUDCASE(1800)
BAUDCASE(2400)
BAUDCASE(4800)
BAUDCASE(9600)
BAUDCASE(19200)
BAUDCASE(38400)
BAUDCASE(57600)
BAUDCASE(115200)
BAUDCASE(230400)
BAUDCASE(460800)
BAUDCASE(500000)
BAUDCASE(576000)
BAUDCASE(921600)
BAUDCASE(1000000)
BAUDCASE(1152000)
BAUDCASE(1500000)
BAUDCASE(2000000)
BAUDCASE(2500000)
BAUDCASE(3000000)
BAUDCASE(3500000)
BAUDCASE(4000000)
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Invalid BaudRate: {}", baud));
return;
}
int err = cfsetospeed(&port->tty, static_cast<speed_t>(port->baudRate));
if (err < 0) {
*status = errno;
return;
}
err = cfsetispeed(&port->tty, static_cast<speed_t>(port->baudRate));
if (err < 0) {
*status = errno;
return;
}
err = tcsetattr(port->portId, TCSANOW, &port->tty);
if (err < 0) {
*status = errno;
}
}
void HAL_SetSerialDataBits(HAL_SerialPortHandle handle, int32_t bits,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
int bitFlag = -1;
switch (bits) {
case 5:
bitFlag = CS5;
break;
case 6:
bitFlag = CS6;
break;
case 7:
bitFlag = CS7;
break;
case 8:
bitFlag = CS8;
break;
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Invalid data bits: {}", bits));
return;
}
port->tty.c_cflag &= ~CSIZE;
port->tty.c_cflag |= bitFlag;
int err = tcsetattr(port->portId, TCSANOW, &port->tty);
if (err < 0) {
*status = errno;
}
}
void HAL_SetSerialParity(HAL_SerialPortHandle handle, int32_t parity,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
switch (parity) {
case 0: // None
port->tty.c_cflag &= ~PARENB;
port->tty.c_cflag &= ~CMSPAR;
break;
case 1: // Odd
port->tty.c_cflag |= PARENB;
port->tty.c_cflag &= ~CMSPAR;
port->tty.c_cflag &= ~PARODD;
break;
case 2: // Even
port->tty.c_cflag |= PARENB;
port->tty.c_cflag &= ~CMSPAR;
port->tty.c_cflag |= PARODD;
break;
case 3: // Mark
port->tty.c_cflag |= PARENB;
port->tty.c_cflag |= CMSPAR;
port->tty.c_cflag |= PARODD;
break;
case 4: // Space
port->tty.c_cflag |= PARENB;
port->tty.c_cflag |= CMSPAR;
port->tty.c_cflag &= ~PARODD;
break;
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Invalid parity bits: {}", parity));
return;
}
int err = tcsetattr(port->portId, TCSANOW, &port->tty);
if (err < 0) {
*status = errno;
}
}
void HAL_SetSerialStopBits(HAL_SerialPortHandle handle, int32_t stopBits,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
switch (stopBits) {
case 10: // 1
port->tty.c_cflag &= ~CSTOPB;
break;
case 15: // 1.5
case 20: // 2
port->tty.c_cflag |= CSTOPB;
break;
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Invalid stop bits: {}", stopBits));
return;
}
int err = tcsetattr(port->portId, TCSANOW, &port->tty);
if (err < 0) {
*status = errno;
}
}
void HAL_SetSerialWriteMode(HAL_SerialPortHandle handle, int32_t mode,
int32_t* status) {
// This seems to be a no op on the NI serial port driver
}
void HAL_SetSerialFlowControl(HAL_SerialPortHandle handle, int32_t flow,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
switch (flow) {
case 0:
port->tty.c_cflag &= ~CRTSCTS;
break;
case 1:
port->tty.c_cflag &= ~CRTSCTS;
port->tty.c_iflag &= IXON | IXOFF;
break;
case 2:
port->tty.c_cflag |= CRTSCTS;
break;
default:
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(status, fmt::format("Invalid fc bits: {}", flow));
return;
}
int err = tcsetattr(port->portId, TCSANOW, &port->tty);
if (err < 0) {
*status = errno;
}
}
void HAL_SetSerialTimeout(HAL_SerialPortHandle handle, double timeout,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
port->timeout = timeout;
port->tty.c_cc[VTIME] = static_cast<int>(timeout * 10);
int err = tcsetattr(port->portId, TCSANOW, &port->tty);
if (err < 0) {
*status = errno;
}
}
void HAL_EnableSerialTermination(HAL_SerialPortHandle handle, char terminator,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
port->termination = true;
port->terminationChar = terminator;
}
void HAL_DisableSerialTermination(HAL_SerialPortHandle handle,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
port->termination = false;
}
void HAL_SetSerialReadBufferSize(HAL_SerialPortHandle handle, int32_t size,
int32_t* status) {
// NO OP currently
}
void HAL_SetSerialWriteBufferSize(HAL_SerialPortHandle handle, int32_t size,
int32_t* status) {
// NO OP currently
}
int32_t HAL_GetSerialBytesReceived(HAL_SerialPortHandle handle,
int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return -1;
}
int bytes = 0;
int err = ioctl(port->portId, FIONREAD, &bytes);
if (err < 0) {
*status = errno;
}
return bytes;
}
int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count,
int32_t* status) {
// Don't do anything if 0 bytes were requested
if (count == 0) {
return 0;
}
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return -1;
}
int n = 0, loc = 0;
char buf = '\0';
std::memset(buffer, '\0', count);
*status = 0;
do {
n = read(port->portId, &buf, 1);
if (n == 1) {
buffer[loc] = buf;
loc++;
// If buffer is full, return
if (loc == count) {
return loc;
}
// If terminating, and termination was hit return;
if (port->termination && buf == port->terminationChar) {
return loc;
}
} else if (n == -1) {
// ERROR
*status = errno;
return loc;
} else {
// If nothing read, timeout
return loc;
}
} while (true);
}
int32_t HAL_WriteSerial(HAL_SerialPortHandle handle, const char* buffer,
int32_t count, int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return -1;
}
int written = 0, spot = 0;
do {
written = write(port->portId, buffer + spot, count - spot);
if (written < 0) {
*status = errno;
return spot;
}
spot += written;
} while (spot < count);
return spot;
}
void HAL_FlushSerial(HAL_SerialPortHandle handle, int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
int err = tcdrain(port->portId);
if (err < 0) {
*status = errno;
}
}
void HAL_ClearSerial(HAL_SerialPortHandle handle, int32_t* status) {
auto port = serialPortHandles->Get(handle);
if (!port) {
*status = HAL_HANDLE_ERROR;
return;
}
int err = tcflush(port->portId, TCIOFLUSH);
if (err < 0) {
*status = errno;
}
}
} // extern "C"

53
hal/src/main/native/athena/SimDevice.cpp
View File

@@ -1,53 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/SimDevice.h"
extern "C" {
HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) {
return 0;
}
void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {}
const char* HAL_GetSimDeviceName(HAL_SimDeviceHandle handle) {
return "";
}
HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
const char* name, int32_t direction,
const struct HAL_Value* initialValue) {
return 0;
}
HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
const char* name, int32_t direction,
int32_t numOptions,
const char** options,
int32_t initialValue) {
return 0;
}
HAL_SimValueHandle HAL_CreateSimValueEnumDouble(
HAL_SimDeviceHandle device, const char* name, int32_t direction,
int32_t numOptions, const char** options, const double* optionValues,
int32_t initialValue) {
return 0;
}
void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value) {
value->type = HAL_UNASSIGNED;
}
void HAL_SetSimValue(HAL_SimValueHandle handle, const struct HAL_Value* value) {
}
void HAL_ResetSimValue(HAL_SimValueHandle handle) {}
hal::SimDevice::SimDevice(const char* name, int index) {}
hal::SimDevice::SimDevice(const char* name, int index, int channel) {}
} // extern "C"

90
hal/src/main/native/athena/Threads.cpp
View File

@@ -1,90 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/Threads.h"
#include <pthread.h>
#include <sched.h>
#include "hal/Errors.h"
namespace hal::init {
void InitializeThreads() {}
} // namespace hal::init
extern "C" {
int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
int32_t* status) {
sched_param sch;
int policy;
int success = pthread_getschedparam(
*reinterpret_cast<const pthread_t*>(handle), &policy, &sch);
if (success == 0) {
*status = 0;
} else {
*status = HAL_THREAD_PRIORITY_ERROR;
return -1;
}
if (policy == SCHED_FIFO || policy == SCHED_RR) {
*isRealTime = true;
return sch.sched_priority;
} else {
*isRealTime = false;
// 0 is the only supported priority for non-real-time
return 0;
}
}
int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status) {
auto thread = pthread_self();
return HAL_GetThreadPriority(&thread, isRealTime, status);
}
HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
int32_t priority, int32_t* status) {
if (handle == nullptr) {
*status = NULL_PARAMETER;
return false;
}
int scheduler = realTime ? SCHED_FIFO : SCHED_OTHER;
if (realTime) {
// We don't support setting priorities for non RT threads
// so we don't need to check for proper range
if (priority < sched_get_priority_min(scheduler) ||
priority > sched_get_priority_max(scheduler)) {
*status = HAL_THREAD_PRIORITY_RANGE_ERROR;
return false;
}
}
sched_param sch;
int policy;
pthread_getschedparam(*reinterpret_cast<const pthread_t*>(handle), &policy,
&sch);
if (scheduler == SCHED_FIFO || scheduler == SCHED_RR) {
sch.sched_priority = priority;
} else {
// Only need to set 0 priority for non RT thread
sch.sched_priority = 0;
}
if (pthread_setschedparam(*reinterpret_cast<const pthread_t*>(handle),
scheduler, &sch)) {
*status = HAL_THREAD_PRIORITY_ERROR;
return false;
} else {
*status = 0;
return true;
}
}
HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
int32_t* status) {
auto thread = pthread_self();
return HAL_SetThreadPriority(&thread, realTime, priority, status);
}
} // extern "C"

355
hal/src/main/native/athena/cpp/SerialHelper.cpp
View File

@@ -1,355 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/cpp/SerialHelper.h"
#include <algorithm>
#include <cstdio>
#include <cstring>
#include <string>
#include <string_view>
#include <vector>
#include <wpi/StringExtras.h>
#include <wpi/fs.h>
#include "hal/Errors.h"
#include "visa/visa.h"
constexpr const char* OnboardResourceVISA = "ASRL1::INSTR";
constexpr const char* MxpResourceVISA = "ASRL2::INSTR";
constexpr const char* OnboardResourceOS = "/dev/ttyS0";
constexpr const char* MxpResourceOS = "/dev/ttyS1";
namespace hal {
std::string SerialHelper::m_usbNames[2]{"", ""};
wpi::mutex SerialHelper::m_nameMutex;
SerialHelper::SerialHelper() {
viOpenDefaultRM(reinterpret_cast<ViSession*>(&m_resourceHandle));
}
std::string SerialHelper::GetVISASerialPortName(HAL_SerialPort port,
int32_t* status) {
if (port == HAL_SerialPort::HAL_SerialPort_Onboard) {
return OnboardResourceVISA;
} else if (port == HAL_SerialPort::HAL_SerialPort_MXP) {
return MxpResourceVISA;
}
QueryHubPaths(status);
// If paths are empty or status error, return error
if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
m_sortedHubPath.empty()) {
*status = HAL_SERIAL_PORT_NOT_FOUND;
return "";
}
int32_t visaIndex = GetIndexForPort(port, status);
if (visaIndex == -1) {
*status = HAL_SERIAL_PORT_NOT_FOUND;
return "";
// Error
} else {
return std::string{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 std::string{m_osResource[osIndex].str()};
}
}
std::vector<std::string> SerialHelper::GetVISASerialPortList(int32_t* status) {
std::vector<std::string> retVec;
// Always add 2 onboard ports
retVec.emplace_back(OnboardResourceVISA);
retVec.emplace_back(MxpResourceVISA);
QueryHubPaths(status);
// If paths are empty or status error, return only onboard list
if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
m_sortedHubPath.empty()) {
*status = 0;
return retVec;
}
for (auto& i : m_visaResource) {
retVec.emplace_back(i.str());
}
return retVec;
}
std::vector<std::string> SerialHelper::GetOSSerialPortList(int32_t* status) {
std::vector<std::string> retVec;
// Always add 2 onboard ports
retVec.emplace_back(OnboardResourceOS);
retVec.emplace_back(MxpResourceOS);
QueryHubPaths(status);
// If paths are empty or status error, return only onboard list
if (*status != 0 || m_visaResource.empty() || m_osResource.empty() ||
m_sortedHubPath.empty()) {
*status = 0;
return retVec;
}
for (auto& i : m_osResource) {
retVec.emplace_back(i.str());
}
return retVec;
}
void SerialHelper::SortHubPathVector() {
m_sortedHubPath.clear();
m_sortedHubPath = m_unsortedHubPath;
std::sort(m_sortedHubPath.begin(), m_sortedHubPath.end(),
[](const wpi::SmallVectorImpl<char>& lhs,
const wpi::SmallVectorImpl<char>& rhs) -> int {
std::string_view lhsRef(lhs.begin(), lhs.size());
std::string_view rhsRef(rhs.begin(), rhs.size());
return lhsRef.compare(rhsRef);
});
}
void SerialHelper::CoiteratedSort(
wpi::SmallVectorImpl<wpi::SmallString<16>>& vec) {
wpi::SmallVector<wpi::SmallString<16>, 4> sortedVec;
for (auto& str : m_sortedHubPath) {
for (size_t i = 0; i < m_unsortedHubPath.size(); i++) {
if (wpi::equals(std::string_view{m_unsortedHubPath[i].begin(),
m_unsortedHubPath[i].size()},
std::string_view{str.begin(), str.size()})) {
sortedVec.push_back(vec[i]);
break;
}
}
}
vec.swap(sortedVec);
}
void SerialHelper::QueryHubPaths(int32_t* status) {
// VISA resource matching string
const char* str = "?*";
// Items needed for VISA
ViUInt32 retCnt = 0;
ViFindList viList = 0;
ViChar desc[VI_FIND_BUFLEN];
*status = viFindRsrc(m_resourceHandle, const_cast<char*>(str), &viList,
&retCnt, desc);
if (*status < 0) {
// Handle the bad status elsewhere
// Note let positive statii (warnings) continue
goto done;
}
// Status might be positive, so reset it to 0
*status = 0;
// Storage buffer for Visa call
char osName[256];
// Loop through all returned VISA objects.
// Increment the internal VISA ptr every loop
for (size_t i = 0; i < retCnt; i++, viFindNext(viList, desc)) {
// Ignore any matches to the 2 onboard ports
if (std::strcmp(OnboardResourceVISA, desc) == 0 ||
std::strcmp(MxpResourceVISA, desc) == 0) {
continue;
}
// Open the resource, grab its interface name, and close it.
ViSession vSession;
*status = viOpen(m_resourceHandle, desc, VI_NULL, VI_NULL, &vSession);
if (*status < 0) {
continue;
}
*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 separate close variable so we can check
ViStatus closeStatus = viClose(vSession);
if (*status < 0) {
continue;
}
if (closeStatus < 0) {
continue;
}
*status = 0;
// split until (/dev/
std::string_view devNameRef = wpi::split(osName, "(/dev/").second;
// String not found, continue
if (wpi::equals(devNameRef, "")) {
continue;
}
// Split at )
std::string_view matchString = wpi::split(devNameRef, ')').first;
if (wpi::equals(matchString, devNameRef)) {
continue;
}
// Search directories to get a list of system accessors
// The directories we need are not symbolic, so we can safely
// disable symbolic links.
std::error_code ec;
for (auto& p : fs::recursive_directory_iterator("/sys/devices/soc0", ec)) {
if (ec) {
break;
}
std::string path = p.path();
if (path.find("amba") == std::string::npos) {
continue;
}
if (path.find("usb") == std::string::npos) {
continue;
}
if (path.find(matchString) == std::string::npos) {
continue;
}
wpi::SmallVector<std::string_view, 16> pathSplitVec;
// Split path into individual directories
wpi::split(path, '/', -1, false,
[&](auto part) { pathSplitVec.emplace_back(part); });
// Find each individual item index
int findusb = -1;
int findtty = -1;
int findregex = -1;
for (size_t i = 0; i < pathSplitVec.size(); i++) {
if (findusb == -1 && wpi::equals(pathSplitVec[i], "usb1")) {
findusb = i;
}
if (findtty == -1 && wpi::equals(pathSplitVec[i], "tty")) {
findtty = i;
}
if (findregex == -1 && wpi::equals(pathSplitVec[i], 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(
std::string_view{pathSplitVec[hubIndex - 2]});
m_visaResource.emplace_back(desc);
m_osResource.emplace_back(
wpi::split(wpi::split(osName, "(").second, ")").first);
break;
}
}
SortHubPathVector();
CoiteratedSort(m_visaResource);
CoiteratedSort(m_osResource);
done:
viClose(viList);
}
int32_t SerialHelper::GetIndexForPort(HAL_SerialPort port, int32_t* status) {
// Hold lock whenever we're using the names array
std::scoped_lock lock(m_nameMutex);
std::string portString = m_usbNames[port - 2];
wpi::SmallVector<int32_t, 4> indices;
// If port has not been assigned, find the one to assign
if (portString.empty()) {
for (size_t i = 0; i < 2; i++) {
// Remove all used ports
auto idx = std::find_if(
m_sortedHubPath.begin(), m_sortedHubPath.end(),
[&](const auto& s) { return wpi::equals(s, 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 (wpi::equals(m_sortedHubPath[i], portString)) {
retIndex = i;
break;
}
}
return retIndex;
}
} // namespace hal

25
hal/src/main/native/athena/mockdata/AccelerometerData.cpp
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/AccelerometerData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetAccelerometerData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Accelerometer##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Active, false)
DEFINE_CAPI(HAL_AccelerometerRange, Range, HAL_AccelerometerRange_k2G)
DEFINE_CAPI(double, X, 0)
DEFINE_CAPI(double, Y, 0)
DEFINE_CAPI(double, Z, 0)
void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

44
hal/src/main/native/athena/mockdata/AddressableLEDData.cpp
View File

@@ -1,44 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/AddressableLEDData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) {
return 0;
}
void HALSIM_ResetAddressableLEDData(int32_t index) {}
int32_t HALSIM_GetAddressableLEDData(int32_t index,
struct HAL_AddressableLEDData* data) {
return 0;
}
void HALSIM_SetAddressableLEDData(int32_t index,
const struct HAL_AddressableLEDData* data,
int32_t length) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(int32_t, OutputPort, 0)
DEFINE_CAPI(int32_t, Length, 0)
DEFINE_CAPI(HAL_Bool, Running, false)
#undef DEFINE_CAPI
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME)
DEFINE_CAPI(HAL_ConstBufferCallback, Data, data)
void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

23
hal/src/main/native/athena/mockdata/AnalogGyroData.cpp
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/AnalogGyroData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetAnalogGyroData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogGyro##CAPINAME, RETURN)
DEFINE_CAPI(double, Angle, 0)
DEFINE_CAPI(double, Rate, 0)
DEFINE_CAPI(HAL_Bool, Initialized, false)
void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

32
hal/src/main/native/athena/mockdata/AnalogInData.cpp
View File

@@ -1,32 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/AnalogInData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetAnalogInData(int32_t index) {}
HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index) {
return 0;
}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogIn##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(int32_t, AverageBits, 0)
DEFINE_CAPI(int32_t, OversampleBits, 0)
DEFINE_CAPI(double, Voltage, 0)
DEFINE_CAPI(HAL_Bool, AccumulatorInitialized, false)
DEFINE_CAPI(int64_t, AccumulatorValue, 0)
DEFINE_CAPI(int64_t, AccumulatorCount, 0)
DEFINE_CAPI(int32_t, AccumulatorCenter, 0)
DEFINE_CAPI(int32_t, AccumulatorDeadband, 0)
void HALSIM_RegisterAnalogInAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {}
} // extern "C"

22
hal/src/main/native/athena/mockdata/AnalogOutData.cpp
View File

@@ -1,22 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/AnalogOutData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetAnalogOutData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogOut##CAPINAME, RETURN)
DEFINE_CAPI(double, Voltage, 0)
DEFINE_CAPI(HAL_Bool, Initialized, false)
void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {
}
} // extern "C"

30
hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp
View File

@@ -1,30 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/AnalogTriggerData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) {
return 0;
}
void HALSIM_ResetAnalogTriggerData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogTrigger##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(double, TriggerLowerBound, 0)
DEFINE_CAPI(double, TriggerUpperBound, 0)
DEFINE_CAPI(HALSIM_AnalogTriggerMode, TriggerMode,
HALSIM_AnalogTriggerUnassigned)
void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

38
hal/src/main/native/athena/mockdata/CTREPCMData.cpp
View File

@@ -1,38 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/CTREPCMData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetCTREPCMData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, CTREPCM##CAPINAME, RETURN)
HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, CTREPCMSolenoidOutput,
false)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(HAL_Bool, CompressorOn, false)
DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false)
DEFINE_CAPI(HAL_Bool, PressureSwitch, false)
DEFINE_CAPI(double, CompressorCurrent, 0)
void HALSIM_GetCTREPCMAllSolenoids(int32_t index, uint8_t* values) {
*values = 0;
}
void HALSIM_SetCTREPCMAllSolenoids(int32_t index, uint8_t values) {}
void HALSIM_RegisterCTREPCMAllNonSolenoidCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
void HALSIM_RegisterCTREPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

22
hal/src/main/native/athena/mockdata/CanDataInternal.cpp
View File

@@ -1,22 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/CanData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetCanData(void) {}
#define DEFINE_CAPI(TYPE, CAPINAME) \
HAL_SIMCALLBACKREGISTRY_STUB_CAPI_NOINDEX(TYPE, HALSIM, Can##CAPINAME)
DEFINE_CAPI(HAL_CAN_SendMessageCallback, SendMessage)
DEFINE_CAPI(HAL_CAN_ReceiveMessageCallback, ReceiveMessage)
DEFINE_CAPI(HAL_CAN_OpenStreamSessionCallback, OpenStream)
DEFINE_CAPI(HAL_CAN_CloseStreamSessionCallback, CloseStream)
DEFINE_CAPI(HAL_CAN_ReadStreamSessionCallback, ReadStream)
DEFINE_CAPI(HAL_CAN_GetCANStatusCallback, GetCANStatus)
} // extern "C"

27
hal/src/main/native/athena/mockdata/DIOData.cpp
View File

@@ -1,27 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/DIOData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetDIOData(int32_t index) {}
HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) {
return 0;
}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DIO##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(HAL_Bool, Value, false)
DEFINE_CAPI(double, PulseLength, 0)
DEFINE_CAPI(HAL_Bool, IsInput, false)
DEFINE_CAPI(int32_t, FilterIndex, 0)
void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {}
} // extern "C"

27
hal/src/main/native/athena/mockdata/DigitalPWMData.cpp
View File

@@ -1,27 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/DigitalPWMData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) {
return 0;
}
void HALSIM_ResetDigitalPWMData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DigitalPWM##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(double, DutyCycle, 0)
DEFINE_CAPI(int32_t, Pin, 0)
void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

123
hal/src/main/native/athena/mockdata/DriverStationData.cpp
View File

@@ -1,123 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/DriverStationData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetDriverStationData(void) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, HALSIM, DriverStation##CAPINAME, \
RETURN)
DEFINE_CAPI(HAL_Bool, Enabled, false)
DEFINE_CAPI(HAL_Bool, Autonomous, false)
DEFINE_CAPI(HAL_Bool, Test, false)
DEFINE_CAPI(HAL_Bool, EStop, false)
DEFINE_CAPI(HAL_Bool, FmsAttached, false)
DEFINE_CAPI(HAL_Bool, DsAttached, false)
DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId,
HAL_AllianceStationID_kRed1)
DEFINE_CAPI(double, MatchTime, 0)
#undef DEFINE_CAPI
#define DEFINE_CAPI(name, data) \
int32_t HALSIM_RegisterJoystick##name##Callback( \
int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
HAL_Bool initialNotify) { \
return 0; \
} \
\
void HALSIM_CancelJoystick##name##Callback(int32_t uid) {} \
\
void HALSIM_GetJoystick##name(int32_t joystickNum, HAL_Joystick##name* d) {} \
\
void HALSIM_SetJoystick##name(int32_t joystickNum, \
const HAL_Joystick##name* d) {}
DEFINE_CAPI(Axes, axes)
DEFINE_CAPI(POVs, povs)
DEFINE_CAPI(Buttons, buttons)
DEFINE_CAPI(Descriptor, descriptor)
int32_t HALSIM_RegisterJoystickOutputsCallback(
int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelJoystickOutputsCallback(int32_t uid) {}
void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
int32_t* leftRumble, int32_t* rightRumble) {}
void HALSIM_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
int32_t leftRumble, int32_t rightRumble) {}
int32_t HALSIM_RegisterMatchInfoCallback(HAL_MatchInfoCallback callback,
void* param, HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelMatchInfoCallback(int32_t uid) {}
void HALSIM_GetMatchInfo(HAL_MatchInfo* info) {}
void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) {}
int32_t HALSIM_RegisterDriverStationNewDataCallback(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelDriverStationNewDataCallback(int32_t uid) {}
void HALSIM_NotifyDriverStationNewData(void) {}
void HALSIM_SetJoystickButton(int32_t stick, int32_t button, HAL_Bool state) {}
void HALSIM_SetJoystickAxis(int32_t stick, int32_t axis, double value) {}
void HALSIM_SetJoystickPOV(int32_t stick, int32_t pov, int32_t value) {}
void HALSIM_SetJoystickButtonsValue(int32_t stick, uint32_t buttons) {}
void HALSIM_SetJoystickAxisCount(int32_t stick, int32_t count) {}
void HALSIM_SetJoystickPOVCount(int32_t stick, int32_t count) {}
void HALSIM_SetJoystickButtonCount(int32_t stick, int32_t count) {}
void HALSIM_GetJoystickCounts(int32_t stick, int32_t* axisCount,
int32_t* buttonCount, int32_t* povCount) {
*axisCount = 0;
*buttonCount = 0;
*povCount = 0;
}
void HALSIM_SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {}
void HALSIM_SetJoystickType(int32_t stick, int32_t type) {}
void HALSIM_SetJoystickName(int32_t stick, const struct WPI_String* name) {}
void HALSIM_SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type) {}
void HALSIM_SetGameSpecificMessage(const struct WPI_String* message) {}
void HALSIM_SetEventName(const struct WPI_String* name) {}
void HALSIM_SetMatchType(HAL_MatchType type) {}
void HALSIM_SetMatchNumber(int32_t matchNumber) {}
void HALSIM_SetReplayNumber(int32_t replayNumber) {}
void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

36
hal/src/main/native/athena/mockdata/DutyCycleData.cpp
View File

@@ -1,36 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/DutyCycleData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) {
return 0;
}
void HALSIM_ResetDutyCycleData(int32_t index) {}
int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) {
return 0;
}
HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) {
return 0;
}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(int32_t, Frequency, 0)
DEFINE_CAPI(double, Output, 0)
void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {
}
} // extern "C"

56
hal/src/main/native/athena/mockdata/EncoderData.cpp
View File

@@ -1,56 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/EncoderData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
int32_t HALSIM_FindEncoderForChannel(int32_t channel) {
return 0;
}
void HALSIM_ResetEncoderData(int32_t index) {}
int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index) {
return 0;
}
int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index) {
return 0;
}
HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index) {
return 0;
}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Encoder##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(int32_t, Count, 0)
DEFINE_CAPI(double, Period, 0)
DEFINE_CAPI(HAL_Bool, Reset, false)
DEFINE_CAPI(double, MaxPeriod, 0)
DEFINE_CAPI(HAL_Bool, Direction, false)
DEFINE_CAPI(HAL_Bool, ReverseDirection, false)
DEFINE_CAPI(int32_t, SamplesToAverage, 0)
DEFINE_CAPI(double, DistancePerPulse, 0)
void HALSIM_SetEncoderDistance(int32_t index, double distance) {}
double HALSIM_GetEncoderDistance(int32_t index) {
return 0;
}
void HALSIM_SetEncoderRate(int32_t index, double rate) {}
double HALSIM_GetEncoderRate(int32_t index) {
return 0;
}
void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {}
} // extern "C"

24
hal/src/main/native/athena/mockdata/I2CData.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/I2CData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetI2CData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, I2C##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
#undef DEFINE_CAPI
#define DEFINE_CAPI(TYPE, CAPINAME) \
HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, I2C##CAPINAME)
DEFINE_CAPI(HAL_BufferCallback, Read)
DEFINE_CAPI(HAL_ConstBufferCallback, Write)
} // extern "C"

53
hal/src/main/native/athena/mockdata/MockHooks.cpp
View File

@@ -1,53 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/MockHooks.h"
extern "C" {
void HALSIM_SetRuntimeType(HAL_RuntimeType type) {}
void HALSIM_WaitForProgramStart(void) {}
void HALSIM_SetProgramStarted(void) {}
HAL_Bool HALSIM_GetProgramStarted(void) {
return false;
}
void HALSIM_RestartTiming(void) {}
void HALSIM_PauseTiming(void) {}
void HALSIM_ResumeTiming(void) {}
HAL_Bool HALSIM_IsTimingPaused(void) {
return false;
}
void HALSIM_StepTiming(uint64_t delta) {}
void HALSIM_StepTimingAsync(uint64_t delta) {}
void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {}
void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {}
int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
HALSIM_SimPeriodicCallback callback, void* param) {
return 0;
}
void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {}
int32_t HALSIM_RegisterSimPeriodicAfterCallback(
HALSIM_SimPeriodicCallback callback, void* param) {
return 0;
}
void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {}
void HALSIM_CancelAllSimPeriodicCallbacks(void) {}
} // extern "C"

21
hal/src/main/native/athena/mockdata/NotifierData.cpp
View File

@@ -1,21 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/NotifierData.h"
extern "C" {
uint64_t HALSIM_GetNextNotifierTimeout(void) {
return 0;
}
int32_t HALSIM_GetNumNotifiers(void) {
return 0;
}
int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
return 0;
}
} // extern "C"

24
hal/src/main/native/athena/mockdata/PWMData.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/PWMData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetPWMData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PWM##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(int32_t, PulseMicrosecond, 0)
DEFINE_CAPI(double, Speed, 0)
DEFINE_CAPI(double, Position, 0)
DEFINE_CAPI(int32_t, PeriodScale, 0)
DEFINE_CAPI(HAL_Bool, ZeroLatch, false)
void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {}
} // extern "C"

35
hal/src/main/native/athena/mockdata/PowerDistributionData.cpp
View File

@@ -1,35 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/PowerDistributionData.h"
#include "../PortsInternal.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetPowerDistributionData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PowerDistribution##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(double, Temperature, 0)
DEFINE_CAPI(double, Voltage, 0)
HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(double, HALSIM, PowerDistributionCurrent, 0)
void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents,
int length) {
for (int i = 0; i < length; i++) {
currents[i] = 0;
}
}
void HALSIM_SetPowerDistributionAllCurrents(int32_t index,
const double* currents,
int length) {}
void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks(
int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
HAL_Bool initialNotify) {}
} // extern "C"

38
hal/src/main/native/athena/mockdata/REVPHData.cpp
View File

@@ -1,38 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/REVPHData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetREVPHData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, REVPH##CAPINAME, RETURN)
HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput, false)
DEFINE_CAPI(HAL_Bool, Initialized, false)
DEFINE_CAPI(HAL_Bool, CompressorOn, false)
DEFINE_CAPI(HAL_REVPHCompressorConfigType, CompressorConfigType,
HAL_REVPHCompressorConfigType_kDisabled)
DEFINE_CAPI(HAL_Bool, PressureSwitch, false)
DEFINE_CAPI(double, CompressorCurrent, 0)
void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) {
*values = 0;
}
void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) {}
void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

23
hal/src/main/native/athena/mockdata/RelayData.cpp
View File

@@ -1,23 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/RelayData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetRelayData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Relay##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, InitializedForward, false)
DEFINE_CAPI(HAL_Bool, InitializedReverse, false)
DEFINE_CAPI(HAL_Bool, Forward, false)
DEFINE_CAPI(HAL_Bool, Reverse, false)
void HALSIM_RegisterRelayAllCallbacks(int32_t index,
HAL_NotifyCallback callback, void* param,
HAL_Bool initialNotify) {}
} // extern "C"

5
hal/src/main/native/athena/mockdata/Reset.cpp
View File

@@ -1,5 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
extern "C" void HALSIM_ResetAllSimData(void) {}

57
hal/src/main/native/athena/mockdata/RoboRioData.cpp
View File

@@ -1,57 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/RoboRioData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetRoboRioData(void) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, HALSIM, RoboRio##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, FPGAButton, false)
DEFINE_CAPI(double, VInVoltage, 0)
DEFINE_CAPI(double, VInCurrent, 0)
DEFINE_CAPI(double, UserVoltage6V, 0)
DEFINE_CAPI(double, UserCurrent6V, 0)
DEFINE_CAPI(HAL_Bool, UserActive6V, false)
DEFINE_CAPI(double, UserVoltage5V, 0)
DEFINE_CAPI(double, UserCurrent5V, 0)
DEFINE_CAPI(HAL_Bool, UserActive5V, false)
DEFINE_CAPI(double, UserVoltage3V3, 0)
DEFINE_CAPI(double, UserCurrent3V3, 0)
DEFINE_CAPI(HAL_Bool, UserActive3V3, false)
DEFINE_CAPI(int32_t, UserFaults6V, 0)
DEFINE_CAPI(int32_t, UserFaults5V, 0)
DEFINE_CAPI(int32_t, UserFaults3V3, 0)
DEFINE_CAPI(double, BrownoutVoltage, 6.75)
DEFINE_CAPI(double, CPUTemp, 45.0)
DEFINE_CAPI(int32_t, TeamNumber, 0)
DEFINE_CAPI(HAL_RadioLEDState, RadioLEDState, HAL_RadioLED_kOff);
int32_t HALSIM_RegisterRoboRioSerialNumberCallback(
HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelRoboRioSerialNumberCallback(int32_t uid) {}
void HALSIM_GetRoboRioSerialNumber(struct WPI_String* serialNumber) {
WPI_AllocateString(serialNumber, 0);
}
void HALSIM_SetRoboRioSerialNumber(const struct WPI_String* serialNumber) {}
int32_t HALSIM_RegisterRoboRioCommentsCallback(
HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelRoboRioCommentsCallback(int32_t uid) {}
void HALSIM_GetRoboRioComments(struct WPI_String* comments) {
WPI_AllocateString(comments, 0);
}
void HALSIM_SetRoboRioComments(const struct WPI_String* comments) {}
void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
void* param, HAL_Bool initialNotify) {}
} // extern "C"

25
hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/SPIAccelerometerData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetSPIAccelerometerData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPIAccelerometer##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Active, false)
DEFINE_CAPI(int32_t, Range, 0)
DEFINE_CAPI(double, X, 0)
DEFINE_CAPI(double, Y, 0)
DEFINE_CAPI(double, Z, 0)
void HALSIM_RegisterSPIAccelerometerAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

25
hal/src/main/native/athena/mockdata/SPIData.cpp
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/SPIData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetSPIData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
#undef DEFINE_CAPI
#define DEFINE_CAPI(TYPE, CAPINAME) \
HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME)
DEFINE_CAPI(HAL_BufferCallback, Read)
DEFINE_CAPI(HAL_ConstBufferCallback, Write)
DEFINE_CAPI(HAL_SpiReadAutoReceiveBufferCallback, ReadAutoReceivedData)
} // extern "C"

97
hal/src/main/native/athena/mockdata/SimDeviceData.cpp
View File

@@ -1,97 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "hal/simulation/SimDeviceData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_SetSimDeviceEnabled(const char* prefix, HAL_Bool enabled) {}
HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name) {
return false;
}
int32_t HALSIM_RegisterSimDeviceCreatedCallback(
const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {}
int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
HALSIM_SimDeviceCallback callback,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {}
HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) {
return 0;
}
const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) {
return "";
}
HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
return 0;
}
void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
HALSIM_SimDeviceCallback callback) {}
int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
void* param,
HALSIM_SimValueCallback callback,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelSimValueCreatedCallback(int32_t uid) {}
int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
void* param,
HALSIM_SimValueCallback callback,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelSimValueChangedCallback(int32_t uid) {}
int32_t HALSIM_RegisterSimValueResetCallback(HAL_SimValueHandle handle,
void* param,
HALSIM_SimValueCallback callback,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelSimValueResetCallback(int32_t uid) {}
HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
const char* name) {
return 0;
}
void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
HALSIM_SimValueCallback callback) {}
const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
int32_t* numOptions) {
*numOptions = 0;
return nullptr;
}
const double* HALSIM_GetSimValueEnumDoubleValues(HAL_SimValueHandle handle,
int32_t* numOptions) {
*numOptions = 0;
return nullptr;
}
void HALSIM_ResetSimDeviceData(void) {}
} // extern "C"

2014
hal/src/main/native/athena/rev/PDHFrames.cpp
View File

File diff suppressed because it is too large Load Diff

3618
hal/src/main/native/athena/rev/PDHFrames.h
View File

File diff suppressed because it is too large Load Diff

2071
hal/src/main/native/athena/rev/PHFrames.cpp
View File

File diff suppressed because it is too large Load Diff

3816
hal/src/main/native/athena/rev/PHFrames.h
View File

File diff suppressed because it is too large Load Diff