hal/src/main/native/athena/DigitalInternal.cpp

// 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 <thread>

#include <FRC_NetworkCommunication/LoadOut.h>
#include <wpi/mutex.h>

#include "ConstantsInternal.h"
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/AnalogTrigger.h"
#include "hal/ChipObject.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
#include "hal/cpp/UnsafeDIO.h"

namespace hal {

std::unique_ptr<tDIO> digitalSystem;
std::unique_ptr<tRelay> relaySystem;
std::unique_ptr<tPWM> pwmSystem;
std::unique_ptr<tSPI> spiSystem;

// Create a mutex to protect changes to the digital output values
wpi::mutex digitalDIOMutex;

DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
                      kNumDigitalChannels + kNumPWMHeaders>*
    digitalChannelHandles;

namespace init {
void InitializeDigitalInternal() {
  static DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
                               kNumDigitalChannels + kNumPWMHeaders>
      dcH;
  digitalChannelHandles = &dcH;
}
}  // namespace init

namespace detail {
wpi::mutex& UnsafeGetDIOMutex() {
  return digitalDIOMutex;
}
tDIO* UnsafeGetDigialSystem() {
  return digitalSystem.get();
}
int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status) {
  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
  if (port == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }
  tDIO::tDO output;
  output.value = 0;
  if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
    output.SPIPort = (1u << remapSPIChannel(port->channel));
  } else if (port->channel < kNumDigitalHeaders) {
    output.Headers = (1u << port->channel);
  } else {
    output.MXP = (1u << remapMXPChannel(port->channel));
  }
  return output.value;
}
}  // namespace detail

void initializeDigital(int32_t* status) {
  hal::init::CheckInit();
  static std::atomic_bool initialized{false};
  static wpi::mutex initializeMutex;
  // Initial check, as if it's true initialization has finished
  if (initialized) {
    return;
  }

  std::scoped_lock lock(initializeMutex);
  // Second check in case another thread was waiting
  if (initialized) {
    return;
  }

  digitalSystem.reset(tDIO::create(status));

  // Relay Setup
  relaySystem.reset(tRelay::create(status));

  // Turn off all relay outputs.
  relaySystem->writeValue_Forward(0, status);
  relaySystem->writeValue_Reverse(0, status);

  // PWM Setup
  pwmSystem.reset(tPWM::create(status));

  // Make sure that the 9403 IONode has had a chance to initialize before
  // continuing.
  while (pwmSystem->readLoopTiming(status) == 0) {
    std::this_thread::yield();
  }

  if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) {
    *status = LOOP_TIMING_ERROR;  // NOTE: Doesn't display the error
  }

  // Calculate the length, in ms, of one DIO loop
  double loopTime = pwmSystem->readLoopTiming(status) /
                    (kSystemClockTicksPerMicrosecond * 1e3);

  pwmSystem->writeConfig_Period(std::lround(kDefaultPwmPeriod / loopTime),
                                status);
  uint16_t minHigh = std::lround(
      (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime);
  pwmSystem->writeConfig_MinHigh(minHigh, status);
  // Ensure that PWM output values are set to OFF
  for (uint8_t pwmIndex = 0; pwmIndex < kNumPWMChannels; pwmIndex++) {
    // Copy of SetPWM
    if (pwmIndex < tPWM::kNumHdrRegisters) {
      pwmSystem->writeHdr(pwmIndex, kPwmDisabled, status);
    } else {
      pwmSystem->writeMXP(pwmIndex - tPWM::kNumHdrRegisters, kPwmDisabled,
                          status);
    }

    // Copy of SetPWMPeriodScale, set to 4x by default.
    if (pwmIndex < tPWM::kNumPeriodScaleHdrElements) {
      pwmSystem->writePeriodScaleHdr(pwmIndex, 3, status);
    } else {
      pwmSystem->writePeriodScaleMXP(
          pwmIndex - tPWM::kNumPeriodScaleHdrElements, 3, status);
    }
  }

  // SPI setup
  spiSystem.reset(tSPI::create(status));

  initialized = true;
}

bool remapDigitalSource(HAL_Handle digitalSourceHandle,
                        HAL_AnalogTriggerType analogTriggerType,
                        uint8_t& channel, uint8_t& module,
                        bool& analogTrigger) {
  if (isHandleType(digitalSourceHandle, HAL_HandleEnum::AnalogTrigger)) {
    // If handle passed, index is not negative
    int32_t index = getHandleIndex(digitalSourceHandle);
    channel = (index << 2) + analogTriggerType;
    module = channel >> 4;
    analogTrigger = true;
    return true;
  } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::DIO)) {
    int32_t index = getHandleIndex(digitalSourceHandle);
    if (index >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
      // channels 10-15, so need to add headers to remap index
      channel = remapSPIChannel(index) + kNumDigitalHeaders;
      module = 0;
    } else if (index >= kNumDigitalHeaders) {
      channel = remapMXPChannel(index);
      module = 1;
    } else {
      channel = index;
      module = 0;
    }
    analogTrigger = false;
    return true;
  } else 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) {});
}