[hal, wpilib] Update Addressable LED support (#8100)

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

@@ -4,85 +4,152 @@
 
 #include "hal/AddressableLED.h"
 
+#include <stdint.h>
+
+#include <cstdio>
 #include <cstring>
 #include <memory>
+#include <thread>
 
 #include <fmt/format.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/RawTopic.h>
 
 #include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "SmartIo.h"
+#include "SystemServerInternal.h"
+#include "hal/AddressableLEDTypes.h"
 #include "hal/Errors.h"
+#include "hal/cpp/fpga_clock.h"
 
 using namespace hal;
 
+#define LEDS_PREFIX "/leds/"
+
+namespace {
+
+constexpr const char* kRawKey = LEDS_PREFIX "raw";
+
+struct AddressableLEDs {
+  explicit AddressableLEDs(nt::NetworkTableInstance inst)
+      : rawPub{inst.GetRawTopic(kRawKey).Publish(
+            "raw", {.periodic = 0.005, .sendAll = true})} {}
+
+  nt::RawPublisher rawPub;
+  uint8_t s_buffer[HAL_kAddressableLEDMaxLength * 3];
+};
+
+static AddressableLEDs* leds;
+
+}  // namespace
+
 namespace hal::init {
-void InitializeAddressableLED() {}
+void InitializeAddressableLED() {
+  static AddressableLEDs leds_static{hal::GetSystemServer()};
+  leds = &leds_static;
+}
 }  // namespace hal::init
 
 extern "C" {
 
 HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
-    HAL_DigitalHandle outputPort, int32_t* status) {
+    int32_t channel, const char* allocationLocation, int32_t* status) {
   hal::init::CheckInit();
 
-  *status = HAL_HANDLE_ERROR;
-  return HAL_kInvalidHandle;
+  if (channel < 0 || channel >= kNumSmartIo) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for AddressableLED",
+                                     0, kNumSmartIo, channel);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DigitalHandle handle;
+
+  auto port = smartIoHandles->Allocate(channel, HAL_HandleEnum::AddressableLED,
+                                       &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(
+          status, "Invalid Index for AddressableLED", 0, kNumSmartIo, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  port->channel = channel;
+
+  *status = port->InitializeMode(SmartIoMode::AddressableLED);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::AddressableLED);
+    return HAL_kInvalidHandle;
+  }
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
 }
 
-void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {}
+void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
+  auto port = smartIoHandles->Get(handle, HAL_HandleEnum::AddressableLED);
+  if (port == nullptr) {
+    return;
+  }
 
-void HAL_SetAddressableLEDColorOrder(HAL_AddressableLEDHandle handle,
-                                     HAL_AddressableLEDColorOrder colorOrder,
-                                     int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  smartIoHandles->Free(handle, HAL_HandleEnum::AddressableLED);
+
+  // 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("AddressableLED handle free timeout");
+      std::fflush(stdout);
+      break;
+    }
+    std::this_thread::yield();
+  }
 }
 
-void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
-                                     HAL_DigitalHandle outputPort,
-                                     int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+void HAL_SetAddressableLEDStart(HAL_AddressableLEDHandle handle, int32_t start,
+                                int32_t* status) {
+  auto port = smartIoHandles->Get(handle, HAL_HandleEnum::AddressableLED);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SetLedStart(start);
 }
 
 void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
                                  int32_t length, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  auto port = smartIoHandles->Get(handle, HAL_HandleEnum::AddressableLED);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  *status = port->SetLedLength(length);
 }
 
-static_assert(sizeof(HAL_AddressableLEDData) == sizeof(uint32_t),
-              "LED Data must be 32 bit");
+static_assert(sizeof(HAL_AddressableLEDData) == 3, "LED Data must be 3 bytes");
 
-void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
-                                 const struct HAL_AddressableLEDData* data,
-                                 int32_t length, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
-}
-
-void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
-                                    int32_t highTime0, int32_t lowTime0,
-                                    int32_t highTime1, int32_t lowTime1,
-                                    int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
-}
-
-void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
-                                   int32_t syncTime, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
-}
-
-void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
-                                   int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
-}
-
-void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
-                                  int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+void HAL_SetAddressableLEDData(int32_t start, int32_t length,
+                               HAL_AddressableLEDColorOrder colorOrder,
+                               const struct HAL_AddressableLEDData* data,
+                               int32_t* status) {
+  if (start < 0 || start >= HAL_kAddressableLEDMaxLength || length < 0 ||
+      (start + length) >= HAL_kAddressableLEDMaxLength) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  // TODO: handle color order
+  std::memcpy(&leds->s_buffer[start * 3], data, length * 3);
+  leds->rawPub.Set(leds->s_buffer);
 }
 }  // extern "C"

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

@@ -29,7 +29,7 @@ constexpr int32_t kNumCTREPDPChannels = 16;
 constexpr int32_t kNumREVPDHModules = 63;
 constexpr int32_t kNumREVPDHChannels = 24;
 constexpr int32_t kNumDutyCycles = 0;
-constexpr int32_t kNumAddressableLEDs = 0;
+constexpr int32_t kNumAddressableLEDs = 6;
 constexpr int32_t kNumREVPHModules = 63;
 constexpr int32_t kNumREVPHChannels = 16;
 

hal/src/main/native/systemcore/SmartIo.cpp

@@ -8,6 +8,8 @@
 
 #include "HALInitializer.h"
 #include "SystemServerInternal.h"
+#include "hal/AddressableLEDTypes.h"
+#include "hal/Errors.h"
 
 namespace hal {
 
@@ -42,6 +44,10 @@ int32_t SmartIo::InitializeMode(SmartIoMode mode) {
       inst.GetIntegerTopic(subTableString + "valset").Publish(options);
   periodSetPublisher =
       inst.GetIntegerTopic(subTableString + "periodset").Publish(options);
+  ledcountPublisher =
+      inst.GetIntegerTopic(subTableString + "ledcount").Publish(options);
+  ledoffsetPublisher =
+      inst.GetIntegerTopic(subTableString + "ledoffset").Publish(options);
 
   currentMode = mode;
   switch (mode) {
@@ -50,6 +56,10 @@ int32_t SmartIo::InitializeMode(SmartIoMode mode) {
     case SmartIoMode::PwmOutput:
       setPublisher.Set(0);
       break;
+    case SmartIoMode::AddressableLED:
+      ledcountPublisher.Set(0);
+      ledoffsetPublisher.Set(0);
+      break;
 
     // These don't need to set any value
     case SmartIoMode::DigitalInput:
@@ -189,4 +199,26 @@ int32_t SmartIo::GetCounter(int32_t* value) {
   return 0;
 }
 
+int32_t SmartIo::SetLedStart(int32_t start) {
+  if (currentMode != SmartIoMode::AddressableLED) {
+    return INCOMPATIBLE_STATE;
+  }
+  if (start < 0 || start >= HAL_kAddressableLEDMaxLength) {
+    return PARAMETER_OUT_OF_RANGE;
+  }
+  ledoffsetPublisher.Set(start);
+  return 0;
+}
+
+int32_t SmartIo::SetLedLength(int32_t length) {
+  if (currentMode != SmartIoMode::AddressableLED) {
+    return INCOMPATIBLE_STATE;
+  }
+  if (length < 0 || length >= HAL_kAddressableLEDMaxLength) {
+    return PARAMETER_OUT_OF_RANGE;
+  }
+  ledcountPublisher.Set(length);
+  return 0;
+}
+
 }  // namespace hal

hal/src/main/native/systemcore/SmartIo.h

@@ -25,6 +25,7 @@ enum class SmartIoMode {
   PwmOutput = 4,
   SingleCounterRising = 5,
   SingleCounterFalling = 6,
+  AddressableLED = 13,
 };
 
 enum class PwmOutputPeriod {
@@ -46,6 +47,9 @@ struct SmartIo {
   nt::IntegerPublisher periodSetPublisher;
   nt::IntegerSubscriber periodGetSubscriber;
 
+  nt::IntegerPublisher ledcountPublisher;
+  nt::IntegerPublisher ledoffsetPublisher;
+
   int32_t InitializeMode(SmartIoMode mode);
   int32_t SwitchDioDirection(bool input);
 
@@ -63,6 +67,9 @@ struct SmartIo {
   int32_t GetAnalogInput(uint16_t* value);
 
   int32_t GetCounter(int32_t* count);
+
+  int32_t SetLedStart(int32_t start);
+  int32_t SetLedLength(int32_t length);
 };
 
 extern DigitalHandleResource<HAL_DigitalHandle, SmartIo, kNumSmartIo>*

hal/src/main/native/systemcore/mockdata/AddressableLEDData.cpp

@@ -8,32 +8,27 @@
 
 extern "C" {
 
-int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) {
-  return 0;
-}
-
 void HALSIM_ResetAddressableLEDData(int32_t index) {}
 
-int32_t HALSIM_GetAddressableLEDData(int32_t index,
+int32_t HALSIM_GetAddressableLEDData(int32_t start, int32_t length,
                                      struct HAL_AddressableLEDData* data) {
   return 0;
 }
 
-void HALSIM_SetAddressableLEDData(int32_t index,
-                                  const struct HAL_AddressableLEDData* data,
-                                  int32_t length) {}
+void HALSIM_SetAddressableLEDData(int32_t start, int32_t length,
+                                  const struct HAL_AddressableLEDData* data) {}
 
 #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, Start, 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 DEFINE_CAPI(TYPE, CAPINAME, RETURN)               \
+  HAL_SIMCALLBACKREGISTRY_STUB_CAPI_NOINDEX(TYPE, HALSIM, \
+                                            AddressableLED##CAPINAME)
 
 DEFINE_CAPI(HAL_ConstBufferCallback, Data, data)