Add AddressableLED (#2092)

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

@@ -0,0 +1,277 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/AddressableLED.h"
+
+#include <cstring>
+
+#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga_HMB.h>
+
+#include "ConstantsInternal.h"
+#include "DigitalInternal.h"
+#include "PortsInternal.h"
+#include "hal/ChipObject.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/handles/LimitedHandleResource.h"
+
+using namespace hal;
+
+constexpr int32_t kMaxStringSize = 5460;
+
+extern "C" {
+NiFpga_Status NiFpga_ClientFunctionCall(NiFpga_Session session, uint32_t group,
+                                        uint32_t functionId,
+                                        const void* inBuffer,
+                                        size_t inBufferSize, void* outBuffer,
+                                        size_t outBufferSize);
+}  // extern "C"
+
+namespace {
+struct AddressableLED {
+  std::unique_ptr<tLED> led;
+  void* ledBuffer;
+  size_t ledBufferSize;
+  int32_t stringLength = 1;
+};
+}  // namespace
+
+static LimitedHandleResource<
+    HAL_AddressableLEDHandle, AddressableLED, kNumAddressableLEDs,
+    HAL_HandleEnum::AddressableLED>* addressableLEDHandles;
+
+namespace hal {
+namespace init {
+void InitializeAddressableLED() {
+  static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
+                               kNumAddressableLEDs,
+                               HAL_HandleEnum::AddressableLED>
+      alH;
+  addressableLEDHandles = &alH;
+}
+}  // namespace init
+}  // namespace hal
+
+// Shim for broken ChipObject function
+static const uint32_t clientFeature_hostMemoryBuffer = 0;
+static const uint32_t hostMemoryBufferFunction_open = 2;
+
+// Input arguments for HMB open
+struct AtomicHMBOpenInputs {
+  const char* memoryName;
+};
+
+// Output arguments for HMB open
+struct AtomicHMBOpenOutputs {
+  size_t size;
+  void* virtualAddress;
+};
+
+static NiFpga_Status OpenHostMemoryBuffer(NiFpga_Session session,
+                                          const char* memoryName,
+                                          void** virtualAddress, size_t* size) {
+  struct AtomicHMBOpenOutputs outputs;
+
+  struct AtomicHMBOpenInputs inputs;
+  inputs.memoryName = memoryName;
+
+  NiFpga_Status retval = NiFpga_ClientFunctionCall(
+      session, clientFeature_hostMemoryBuffer, hostMemoryBufferFunction_open,
+      &inputs, sizeof(struct AtomicHMBOpenInputs), &outputs,
+      sizeof(struct AtomicHMBOpenOutputs));
+  if (NiFpga_IsError(retval)) {
+    return retval;
+  }
+  *virtualAddress = outputs.virtualAddress;
+  if (size != NULL) {
+    *size = outputs.size;
+  }
+  return retval;
+}
+
+extern "C" {
+
+HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
+    HAL_DigitalHandle outputPort, int32_t* status) {
+  auto digitalPort =
+      hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+  if (!digitalPort) {
+    *status = HAL_HANDLE_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 = OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
+                                 &led->ledBufferSize);
+
+  if (*status != 0) {
+    addressableLEDHandles->Free(handle);
+    return HAL_kInvalidHandle;
+  }
+
+  return handle;
+}
+
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
+  addressableLEDHandles->Free(handle);
+}
+
+void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
+                                     HAL_DigitalHandle outputPort,
+                                     int32_t* status) {
+  auto digitalPort =
+      hal::digitalChannelHandles->Get(outputPort, hal::HAL_HandleEnum::PWM);
+
+  if (!digitalPort) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  led->led->writeOutputSelect(digitalPort->channel, status);
+}
+
+void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
+                                 int32_t length, int32_t* status) {
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (length > kMaxStringSize) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+
+  led->led->strobeReset(status);
+
+  while (led->led->readPixelWriteIndex(status) != 0) {
+  }
+
+  if (*status != 0) {
+    return;
+  }
+
+  led->led->writeStringLength(length, status);
+
+  led->stringLength = length;
+}
+
+static_assert(sizeof(HAL_AddressableLEDData) == sizeof(uint32_t),
+              "LED Data must be 32 bit");
+
+void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
+                                 const struct HAL_AddressableLEDData* data,
+                                 int32_t length, int32_t* status) {
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (length > led->stringLength) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+
+  std::memcpy(led->ledBuffer, data, length * sizeof(HAL_AddressableLEDData));
+
+  asm("dmb");
+
+  led->led->strobeLoad(status);
+}
+
+void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
+                                    int32_t lowTime0NanoSeconds,
+                                    int32_t highTime0NanoSeconds,
+                                    int32_t lowTime1NanoSeconds,
+                                    int32_t highTime1NanoSeconds,
+                                    int32_t* status) {
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  led->led->writeLowBitTickTiming(1, highTime0NanoSeconds / 25, status);
+  led->led->writeLowBitTickTiming(0, lowTime0NanoSeconds / 25, status);
+  led->led->writeHighBitTickTiming(1, highTime1NanoSeconds / 25, status);
+  led->led->writeHighBitTickTiming(0, lowTime1NanoSeconds / 25, status);
+}
+
+void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
+                                   int32_t syncTimeMicroSeconds,
+                                   int32_t* status) {
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  led->led->writeSyncTiming(syncTimeMicroSeconds, status);
+}
+
+void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+                                   int32_t* status) {
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  led->led->strobeStart(status);
+}
+
+void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
+                                  int32_t* status) {
+  auto led = addressableLEDHandles->Get(handle);
+  if (!led) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  led->led->strobeAbort(status);
+}
+}  // extern "C"

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -96,6 +96,20 @@ 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.

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

@@ -42,6 +42,7 @@ using namespace hal;
 namespace hal {
 namespace init {
 void InitializeHAL() {
+  InitializeAddressableLED();
   InitializeAccelerometer();
   InitializeAnalogAccumulator();
   InitializeAnalogGyro();

hal/src/main/native/athena/HALInitializer.h

@@ -19,6 +19,7 @@ static inline void CheckInit() {
 }
 
 extern void InitializeAccelerometer();
+extern void InitializeAddressableLED();
 extern void InitializeAnalogAccumulator();
 extern void InitializeAnalogGyro();
 extern void InitializeAnalogInput();

hal/src/main/native/athena/PortsInternal.h

@@ -36,5 +36,6 @@ constexpr int32_t kNumSolenoidChannels = 8;
 constexpr int32_t kNumPDPModules = 63;
 constexpr int32_t kNumPDPChannels = 16;
 constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems;
+constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems;
 
 }  // namespace hal