From b60b2b64bd14f842c3a6ff4ee83f4c6be918d8e9 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Fri, 7 Feb 2025 15:36:41 -0500 Subject: [PATCH] [hal, wpilib] AddressableLED: add support for other color orders (#7102) Many LED strips use different color order (GRB in particular is common). This makes the change at the HAL level. This solves 2 problems; first, no code needs to change in the high level drivers, which was challenging for C++, and second, simulation will behave properly as no conversion is needed. The HAL will accept an array of data objects in the same order no matter what the selected output order is, and will convert before sending it to the FPGA for output. To accomplish this, NEON bulk load/interleave instructions are utilized. The low level implementation (load, store, and alignment functions) come from the Simd Library. The high level implementations are inspired by the image conversion functions in the simd library, but have diverged significantly. Much of the implementation uses templates and inlined functions rather than runtime parameters; This is a trade off between the size of the generated code and the amount of function calls done at runtime. Currently, the entire conversion operation is inlined. --- ThirdPartyNotices.txt | 27 ++ .../edu/wpi/first/hal/AddressableLEDJNI.java | 17 ++ hal/src/main/native/athena/AddressableLED.cpp | 49 +++- .../main/native/athena/AddressableLEDSimd.h | 273 ++++++++++++++++++ hal/src/main/native/athena/simd/simd.h | 174 +++++++++++ .../main/native/cpp/jni/AddressableLEDJNI.cpp | 29 ++ .../main/native/include/hal/AddressableLED.h | 11 + .../native/include/hal/AddressableLEDTypes.h | 19 ++ hal/src/main/native/sim/AddressableLED.cpp | 4 + .../src/main/native/cpp/AddressableLED.cpp | 7 + .../main/native/include/frc/AddressableLED.h | 29 ++ .../edu/wpi/first/wpilibj/AddressableLED.java | 59 +++- 12 files changed, 695 insertions(+), 3 deletions(-) create mode 100644 hal/src/main/native/athena/AddressableLEDSimd.h create mode 100644 hal/src/main/native/athena/simd/simd.h diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index 01e52c7fd3..4acb020f80 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -54,6 +54,7 @@ nanopb wpiutil/src/main/native/thirdparty/nanopb protobuf wpiutil/src/main/native/thirdparty/protobuf mrcal wpical/src/main/native/thirdparty/mrcal libdogleg wpical/src/main/native/thirdparty/libdogleg +Simd hal/src/main/native/athena/simd Additionally, glfw, memory, and nanopb were all modified for use in WPILib. @@ -1702,3 +1703,29 @@ This program is free software: you can redistribute it and/or modify it under th This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. The full text of the license is available at http://www.gnu.org/licenses + +============ +Simd License +============ + +MIT License + +Copyright (c) 2011-2017 Ihar Yermalayeu + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java b/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java index 4d4c1975ad..c14630ff28 100644 --- a/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java @@ -10,6 +10,13 @@ package edu.wpi.first.hal; * @see "hal/AddressableLED.h" */ public class AddressableLEDJNI extends JNIWrapper { + public static final int COLOR_ORDER_RGB = 0; + public static final int COLOR_ORDER_RBG = 1; + public static final int COLOR_ORDER_BGR = 2; + public static final int COLOR_ORDER_BRG = 3; + public static final int COLOR_ORDER_GBR = 4; + public static final int COLOR_ORDER_GRB = 5; + /** * Initialize Addressable LED using a PWM Digital handle. * @@ -27,6 +34,16 @@ public class AddressableLEDJNI extends JNIWrapper { */ public static native void free(int handle); + /** + * Sets the color order for the addressable LED output. The default order is GRB. + * + *

This will take effect on the next call to {@link #setData(int, byte[])}. + * + * @param handle the Addressable LED handle + * @param colorOrder the color order + */ + public static native void setColorOrder(int handle, int colorOrder); + /** * Sets the length of the LED strip. * diff --git a/hal/src/main/native/athena/AddressableLED.cpp b/hal/src/main/native/athena/AddressableLED.cpp index 8aaa2f33b7..f698d59b4e 100644 --- a/hal/src/main/native/athena/AddressableLED.cpp +++ b/hal/src/main/native/athena/AddressableLED.cpp @@ -9,6 +9,7 @@ #include +#include "AddressableLEDSimd.h" #include "ConstantsInternal.h" #include "DigitalInternal.h" #include "FPGACalls.h" @@ -21,6 +22,7 @@ #include "hal/handles/LimitedHandleResource.h" using namespace hal; +using namespace hal::detail; namespace { struct AddressableLED { @@ -28,6 +30,7 @@ struct AddressableLED { void* ledBuffer; size_t ledBufferSize; int32_t stringLength = 1; + HAL_AddressableLEDColorOrder colorOrder = HAL_ALED_GRB; }; } // namespace @@ -47,6 +50,37 @@ void InitializeAddressableLED() { static constexpr const char* HmbName = "HMB_0_LED"; +static void ConvertAndCopyLEDData(void* dst, + const struct HAL_AddressableLEDData* src, + int32_t len, + HAL_AddressableLEDColorOrder order) { + switch (order) { + case HAL_ALED_GRB: + std::memcpy(dst, src, len * sizeof(HAL_AddressableLEDData)); + break; + case HAL_ALED_RGB: + ConvertPixels(reinterpret_cast(src), + reinterpret_cast(dst), len); + break; + case HAL_ALED_RBG: + ConvertPixels(reinterpret_cast(src), + reinterpret_cast(dst), len); + break; + case HAL_ALED_BGR: + ConvertPixels(reinterpret_cast(src), + reinterpret_cast(dst), len); + break; + case HAL_ALED_BRG: + ConvertPixels(reinterpret_cast(src), + reinterpret_cast(dst), len); + break; + case HAL_ALED_GBR: + ConvertPixels(reinterpret_cast(src), + reinterpret_cast(dst), len); + break; + } +} + extern "C" { HAL_AddressableLEDHandle HAL_InitializeAddressableLED( @@ -125,6 +159,19 @@ void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) { addressableLEDHandles->Free(handle); } +void HAL_SetAddressableLEDColorOrder(HAL_AddressableLEDHandle handle, + HAL_AddressableLEDColorOrder colorOrder, + int32_t* status) { + auto led = addressableLEDHandles->Get(handle); + + if (!led) { + *status = HAL_HANDLE_ERROR; + return; + } + + led->colorOrder = colorOrder; +} + void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle, HAL_DigitalHandle outputPort, int32_t* status) { @@ -203,7 +250,7 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle, return; } - std::memcpy(led->ledBuffer, data, length * sizeof(HAL_AddressableLEDData)); + ConvertAndCopyLEDData(led->ledBuffer, data, length, led->colorOrder); asm("dmb"); diff --git a/hal/src/main/native/athena/AddressableLEDSimd.h b/hal/src/main/native/athena/AddressableLEDSimd.h new file mode 100644 index 0000000000..8d3be71420 --- /dev/null +++ b/hal/src/main/native/athena/AddressableLEDSimd.h @@ -0,0 +1,273 @@ +// 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 + +#include "hal/AddressableLEDTypes.h" +#include "simd/simd.h" + +// Timing info +// https://developer.arm.com/documentation/ddi0409/i/instruction-timing/instruction-specific-scheduling/advanced-simd-load-store-instructions?lang=en + +namespace hal::detail { + +using namespace Simd::Neon; + +template +using ConvertFunc = void (*)(T); + +/* + * Conversion funtions perform in-place conversion by swapping elements. + * The names of the functions indicate the wire output (default GRB), + * but the FPGA takes sequences of BGR_. + */ + +template +void ToRGB(T val) { + std::swap(val[1], val[2]); // swap G and R +} + +template +void ToRBG(T val) { + std::swap(val[1], val[2]); // swap G and R + std::swap(val[0], val[2]); // swap B and G +} + +template +void ToBGR(T val) { + std::swap(val[0], val[1]); // swap B and G + std::swap(val[0], val[2]); // swap G and R +} + +template +void ToBRG(T val) { + std::swap(val[0], val[1]); // swap B and G +} + +template +void ToGBR(T val) { + std::swap(val[0], val[2]); // swap B and R +} + +/** + * Copies 16 pixels from src to dst using NEON instructions, converting using + * the provided conversion function. Optimizes based on alignment of input and + * output arrays specified by srcAlign and dstAlign + * @tparam srcAlign whether src is aligned to the size of a NEON register (16 + * bytes) + * @tparam dstAlign whether dst is aligned to the size of a NEON register (16 + * bytes) + * @tparam the conversion function + * @param[in] src The source array + * @param[out] dst the destination array + * @pre src and dst must contain at least 64 bytes (16 pixels) + * @pre if srcAlign is true, src must be 16 byte aligned + * @pre if dstAlign is true, src muts be 16 byte aligned + */ +template Convert> +void ConvertNEON_16(const uint8_t* src, uint8_t* dst) { + uint8x16x4_t pixels = Load4(src); + Convert(pixels.val); + Store4(dst, pixels); +} + +/** + * Copies 8 pixels from src to dst using NEON instructions, converting using + * the provided conversion function. Optimizes based on alignment of input and + * output arrays specified by srcAlign and dstAlign + * @tparam srcAlign whether src is aligned to the size of a NEON register (16 + * bytes) + * @tparam dstAlign whether dst is aligned to the size of a NEON register (16 + * bytes) + * @tparam the conversion function + * @param[in] src The source array + * @param[out] dst the destination array + * @pre src and dst must contain at least 32 bytes (8 pixels) + * @pre if srcAlign is true, src must be 16 byte aligned + * @pre if dstAlign is true, src muts be 16 byte aligned + */ +template Convert> +void ConvertNEON_8(const uint8_t* src, uint8_t* dst) { + uint8x8x4_t pixels = LoadHalf4(src); + Convert(pixels.val); + Store4(dst, pixels); +} + +/** + * Copies 16 pixels from src to dst, converting from GRB (wire order) to order. + * Optimizes based on alignment of input and output arrays specified by srcAlign + * and dstAlign + * @tparam order the color order to convert to + * @tparam srcAlign whether src is aligned to the size of a NEON register (16 + * bytes) + * @tparam dstAlign whether dst is aligned to the size of a NEON register (16 + * bytes) + * @param[in] src The source array + * @param[out] dst the destination array + * @pre src and dst must contain at least 64 bytes (16 pixels) + * @pre if srcAlign is true, src must be 16 byte aligned + * @pre if dstAlign is true, src muts be 16 byte aligned + */ +template +void Convert16Pixels(const uint8_t* src, uint8_t* dst) { + switch (order) { + case HAL_ALED_RGB: + ConvertNEON_16(src, dst); + break; + case HAL_ALED_RBG: + ConvertNEON_16(src, dst); + break; + case HAL_ALED_BGR: + ConvertNEON_16(src, dst); + break; + case HAL_ALED_BRG: + ConvertNEON_16(src, dst); + break; + case HAL_ALED_GBR: + ConvertNEON_16(src, dst); + break; + } +} + +/** + * Copies 8 pixels from src to dst, converting from GRB (wire order) to order. + * Optimizes based on alignment of input and output arrays specified by srcAlign + * and dstAlign + * @tparam order the color order to convert to + * @tparam srcAlign whether src is aligned to the size of a NEON register (16 + * bytes) + * @tparam dstAlign whether dst is aligned to the size of a NEON register (16 + * bytes) + * @param[in] src The source array + * @param[out] dst the destination array + * @pre src and dst must contain at least 32 bytes (8 pixels) + * @pre if srcAlign is true, src must be 16 byte aligned + * @pre if dstAlign is true, src muts be 16 byte aligned + */ +template +void Convert8Pixels(const uint8_t* src, uint8_t* dst) { + switch (order) { + case HAL_ALED_RGB: + ConvertNEON_8(src, dst); + break; + case HAL_ALED_RBG: + ConvertNEON_8(src, dst); + break; + case HAL_ALED_BGR: + ConvertNEON_8(src, dst); + break; + case HAL_ALED_BRG: + ConvertNEON_8(src, dst); + break; + case HAL_ALED_GBR: + ConvertNEON_8(src, dst); + break; + } +} + +/** + * Copies 1 pixel from src to dst, converting from RGB to the specified order. + * @param[in] order the color order to convert to + * @param[in] in the source array + * @param[out] the destination array + * @pre in and out must contain at least 1 pixel (4 bytes). + */ +void Convert1Pixel(HAL_AddressableLEDColorOrder order, const uint8_t* src, + uint8_t* dst) { + uint8_t tmp[4]; + std::memcpy(tmp, src, 4); // Load 4 bytes + // convert based on order + switch (order) { + case HAL_ALED_RGB: + ToRGB(tmp); + break; + case HAL_ALED_RBG: + ToRBG(tmp); + break; + case HAL_ALED_BGR: + ToBGR(tmp); + break; + case HAL_ALED_BRG: + ToBRG(tmp); + break; + case HAL_ALED_GBR: + ToGBR(tmp); + break; + case HAL_ALED_GRB: + break; // this shouldn't ever get hit but compiler + // wants this to be exhaustive + } + std::memcpy(dst, tmp, 4); // Store 4 bytes +} +/** + * Copies len pixels from src to dst, converting from GRB (wire order) to order. + * Optimizes based on alignment of input and output arrays specified by srcAlign + * and dstAlign + * @tparam order the color order to convert to + * @tparam srcAlign whether src is aligned to the size of a NEON register (16 + * bytes) + * @tparam dstAlign whether dst is aligned to the size of a NEON register (16 + * bytes) + * @param[in] src The source array + * @param[out] dst the destination array + * @param[in] len the size (in pixels, len = (size in bytes) / 4) + * @pre src and dst must have at least len*4 capacity in bytes + * @pre if srcAlign is true, src must be 16 byte aligned + * @pre if dstAlign is true, src muts be 16 byte aligned + */ +template +void ConvertPixels(const uint8_t* src, uint8_t* dst, size_t len) { + if (len >= 16) { + constexpr size_t A4 = + A * 4; // Stride of 1 16-pixel conversion operation. (4 NEON registers) + size_t size = len * 4; + size_t aligned = Simd::AlignLo( + size, A4); // number of bytes we can copy with whole 16-pixel strides + for (size_t i = 0; i < aligned; i += A4) { + Convert16Pixels(src + i, dst + i); + } + if (aligned < size) { + Convert16Pixels( + src + size - A4, + dst + size - A4); // copy last 16 pixels, possibly recopying. + } + } else if (len >= 8) { + // If len between 8 and 16, we can do 1 or 2 8-pixel copies + Convert8Pixels(src, dst); + if (len > 8) { + size_t recopyOffset = (len * 4) - (HA * 4); + Convert8Pixels( + src + recopyOffset, + dst + recopyOffset); // copy last 8 pixels, possibly recopying + } + } else { + // Just copy pixel-by-pixel for <8 + for (size_t i = 0; i < len; i += 4) { + Convert1Pixel(order, src + i, dst + i); + } + } +} + +/** + * Copies pixelCount pixels from src to dst, converting from RGB to the + * specified order + * @tparam order the color order to convert to + * @param src the source array + * @param dst the destination array + * @param pixelCount the number of pixels to convert and copy + */ +template +void ConvertPixels(const uint8_t* src, uint8_t* dst, size_t pixelCount) { + if (Aligned(src) && Aligned(dst)) { + ConvertPixels(src, dst, pixelCount); + } else if (Aligned(src)) { + ConvertPixels(src, dst, pixelCount); + } else if (Aligned(dst)) { + ConvertPixels(src, dst, pixelCount); + } else { + ConvertPixels(src, dst, pixelCount); + } +} +} // namespace hal::detail diff --git a/hal/src/main/native/athena/simd/simd.h b/hal/src/main/native/athena/simd/simd.h new file mode 100644 index 0000000000..397dc54d3d --- /dev/null +++ b/hal/src/main/native/athena/simd/simd.h @@ -0,0 +1,174 @@ +// 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. + +// This file contains modified snippets from the Simd Library by Ihar Yermalayeu +// (http://ermig1979.github.io/Simd). The original source file names are listed +// above each section. +/* + * Simd Library (http://ermig1979.github.io/Simd). + * + * Copyright (c) 2011-2024 Yermalayeu Ihar. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include + +// SimdLib.h +#define SIMD_INLINE inline __attribute__((always_inline)) + +// SimdMemory.h +namespace Simd { +SIMD_INLINE size_t AlignLo(size_t size, size_t align) { + return size & ~(align - 1); +} + +SIMD_INLINE void* AlignLo(const void* ptr, size_t align) { + return reinterpret_cast(reinterpret_cast(ptr) & ~(align - 1)); +} + +SIMD_INLINE bool Aligned(size_t size, size_t align) { + return size == AlignLo(size, align); +} + +SIMD_INLINE bool Aligned(const void* ptr, size_t align) { + return ptr == AlignLo(ptr, align); +} +} // namespace Simd +namespace Simd::Neon { +SIMD_INLINE bool Aligned(size_t size, size_t align = sizeof(uint8x16_t)) { + return Simd::Aligned(size, align); +} + +SIMD_INLINE bool Aligned(const void* ptr, size_t align = sizeof(uint8x16_t)) { + return Simd::Aligned(ptr, align); +} +} // namespace Simd::Neon + +// SimdConst.h +namespace Simd::Neon { +const size_t A = sizeof(uint8x16_t); +const size_t DA = 2 * A; +const size_t QA = 4 * A; +const size_t OA = 8 * A; +const size_t HA = A / 2; +} // namespace Simd::Neon + +// SimdLoad.h +namespace Simd::Neon { +template +SIMD_INLINE uint8x8x4_t LoadHalf4(const uint8_t* p); + +template <> +SIMD_INLINE uint8x8x4_t LoadHalf4(const uint8_t* p) { +#if defined(__GNUC__) && SIMD_NEON_PREFECH_SIZE + __builtin_prefetch(p + SIMD_NEON_PREFECH_SIZE); +#endif + return vld4_u8(p); +} + +template <> +SIMD_INLINE uint8x8x4_t LoadHalf4(const uint8_t* p) { +#if defined(__GNUC__) +#if SIMD_NEON_PREFECH_SIZE + __builtin_prefetch(p + SIMD_NEON_PREFECH_SIZE); +#endif + uint8_t* _p = static_cast(__builtin_assume_aligned(p, 8)); + return vld4_u8(_p); +#elif defined(_MSC_VER) + return vld4_u8_ex(p, 64); +#else + return vld4_u8(p); +#endif +} + +template +SIMD_INLINE uint8x16x4_t Load4(const uint8_t* p); + +template <> +SIMD_INLINE uint8x16x4_t Load4(const uint8_t* p) { +#if defined(__GNUC__) && SIMD_NEON_PREFECH_SIZE + __builtin_prefetch(p + SIMD_NEON_PREFECH_SIZE); +#endif + return vld4q_u8(p); +} + +template <> +SIMD_INLINE uint8x16x4_t Load4(const uint8_t* p) { +#if defined(__GNUC__) +#if SIMD_NEON_PREFECH_SIZE + __builtin_prefetch(p + SIMD_NEON_PREFECH_SIZE); +#endif + uint8_t* _p = static_cast(__builtin_assume_aligned(p, 16)); + return vld4q_u8(_p); +#elif defined(_MSC_VER) + return vld4q_u8_ex(p, 128); +#else + return vld4q_u8(p); +#endif +} + +// SimdStore.h +template +SIMD_INLINE void Store4(uint8_t* p, uint8x16x4_t a); + +template <> +SIMD_INLINE void Store4(uint8_t* p, uint8x16x4_t a) { + vst4q_u8(p, a); +} + +template <> +SIMD_INLINE void Store4(uint8_t* p, uint8x16x4_t a) { +#if defined(__GNUC__) + uint8_t* _p = static_cast(__builtin_assume_aligned(p, 16)); + vst4q_u8(_p, a); +#elif defined(_MSC_VER) + vst4q_u8_ex(p, a, 128); +#else + vst4q_u8(p, a); +#endif +} + +template +SIMD_INLINE void Store4(uint8_t* p, uint8x8x4_t a); + +template <> +SIMD_INLINE void Store4(uint8_t* p, uint8x8x4_t a) { + vst4_u8(p, a); +} + +template <> +SIMD_INLINE void Store4(uint8_t* p, uint8x8x4_t a) { +#if defined(__GNUC__) + uint8_t* _p = static_cast(__builtin_assume_aligned(p, 8)); + vst4_u8(_p, a); +#elif defined(_MSC_VER) + vst4_u8_ex(p, a, 64); +#else + vst4_u8(p, a); +#endif +} + +} // namespace Simd::Neon diff --git a/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp index 1d9017da1c..e7459a2b86 100644 --- a/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp +++ b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp @@ -15,6 +15,19 @@ using namespace wpi::java; static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData)); +static_assert(edu_wpi_first_hal_AddressableLEDJNI_COLOR_ORDER_RGB == + HAL_ALED_RGB); +static_assert(edu_wpi_first_hal_AddressableLEDJNI_COLOR_ORDER_RBG == + HAL_ALED_RBG); +static_assert(edu_wpi_first_hal_AddressableLEDJNI_COLOR_ORDER_BGR == + HAL_ALED_BGR); +static_assert(edu_wpi_first_hal_AddressableLEDJNI_COLOR_ORDER_BRG == + HAL_ALED_BRG); +static_assert(edu_wpi_first_hal_AddressableLEDJNI_COLOR_ORDER_GBR == + HAL_ALED_GBR); +static_assert(edu_wpi_first_hal_AddressableLEDJNI_COLOR_ORDER_GRB == + HAL_ALED_GRB); + extern "C" { /* * Class: edu_wpi_first_hal_AddressableLEDJNI @@ -46,6 +59,22 @@ Java_edu_wpi_first_hal_AddressableLEDJNI_free } } +/* + * Class: edu_wpi_first_hal_AddressableLEDJNI + * Method: setColorOrder + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_AddressableLEDJNI_setColorOrder + (JNIEnv* env, jclass, jint handle, jint colorOrder) +{ + int32_t status = 0; + HAL_SetAddressableLEDColorOrder( + static_cast(handle), + static_cast(colorOrder), &status); + CheckStatus(env, status); +} + /* * Class: edu_wpi_first_hal_AddressableLEDJNI * Method: setLength diff --git a/hal/src/main/native/include/hal/AddressableLED.h b/hal/src/main/native/include/hal/AddressableLED.h index 3181e7e2bb..c404688567 100644 --- a/hal/src/main/native/include/hal/AddressableLED.h +++ b/hal/src/main/native/include/hal/AddressableLED.h @@ -36,6 +36,17 @@ HAL_AddressableLEDHandle HAL_InitializeAddressableLED( */ void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle); +/** + * Sets the color order for the addressable LED output. The default order is + * GRB. This will take effect on the next call to HAL_WriteAddressableLEDData(). + * @param[in] handle the Addressable LED handle + * @param[in] colorOrder the color order + * @param[out] status the error code, or 0 for success + */ +void HAL_SetAddressableLEDColorOrder(HAL_AddressableLEDHandle handle, + HAL_AddressableLEDColorOrder colorOrder, + int32_t* status); + /** * Set the Addressable LED PWM Digital port. * diff --git a/hal/src/main/native/include/hal/AddressableLEDTypes.h b/hal/src/main/native/include/hal/AddressableLEDTypes.h index 48918f4b72..4dad3683c8 100644 --- a/hal/src/main/native/include/hal/AddressableLEDTypes.h +++ b/hal/src/main/native/include/hal/AddressableLEDTypes.h @@ -4,6 +4,7 @@ #pragma once +#include #include /** max length of LED strip supported by FPGA. */ @@ -16,3 +17,21 @@ struct HAL_AddressableLEDData { uint8_t r; ///< red value uint8_t padding; }; + +/** + * Order that color data is sent over the wire. + */ +HAL_ENUM(HAL_AddressableLEDColorOrder) { + HAL_ALED_RGB, + HAL_ALED_RBG, + HAL_ALED_BGR, + HAL_ALED_BRG, + HAL_ALED_GBR, + HAL_ALED_GRB +}; + +#ifdef __cplusplus +constexpr auto format_as(HAL_AddressableLEDColorOrder order) { + return static_cast(order); +} +#endif diff --git a/hal/src/main/native/sim/AddressableLED.cpp b/hal/src/main/native/sim/AddressableLED.cpp index bce3f1ddfd..2bf19d0102 100644 --- a/hal/src/main/native/sim/AddressableLED.cpp +++ b/hal/src/main/native/sim/AddressableLED.cpp @@ -91,6 +91,10 @@ void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) { SimAddressableLEDData[led->index].initialized = false; } +void HAL_SetAddressableLEDColorOrder(HAL_AddressableLEDHandle handle, + HAL_AddressableLEDColorOrder colorOrder, + int32_t* status) {} + void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle, HAL_DigitalHandle outputPort, int32_t* status) { diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp index 7c70033fe3..7e55655664 100644 --- a/wpilibc/src/main/native/cpp/AddressableLED.cpp +++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp @@ -35,6 +35,13 @@ AddressableLED::AddressableLED(int port) : m_port{port} { HAL_Report(HALUsageReporting::kResourceType_AddressableLEDs, port + 1); } +void AddressableLED::SetColorOrder(AddressableLED::ColorOrder order) { + int32_t status = 0; + HAL_SetAddressableLEDColorOrder( + m_handle, static_cast(order), &status); + FRC_CheckErrorStatus(status, "Port {} Color order {}", m_port, order); +} + void AddressableLED::SetLength(int length) { int32_t status = 0; HAL_SetAddressableLEDLength(m_handle, length, &status); diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h index c7af852860..4e5bdefa2e 100644 --- a/wpilibc/src/main/native/include/frc/AddressableLED.h +++ b/wpilibc/src/main/native/include/frc/AddressableLED.h @@ -24,12 +24,27 @@ namespace frc { * By default, the timing supports WS2812B and WS2815 LEDs, but is configurable * using SetBitTiming() * + * Some LEDs use a different color order than the default GRB. The color order + * is configurable using SetColorOrder(). + * *

Only 1 LED driver is currently supported by the roboRIO. However, * multiple LED strips can be connected in series and controlled from the * single driver. */ class AddressableLED { public: + /** + * Order that color data is sent over the wire. + */ + enum ColorOrder { + kRGB = HAL_ALED_RGB, ///< RGB order + kRBG = HAL_ALED_RBG, ///< RBG order + kBGR = HAL_ALED_BGR, ///< BGR order + kBRG = HAL_ALED_BRG, ///< BRG order + kGBR = HAL_ALED_GBR, ///< GBR order + kGRB = HAL_ALED_GRB ///< GRB order. This is the default order. + }; + class LEDData : public HAL_AddressableLEDData { public: LEDData() : LEDData(0, 0, 0) {} @@ -95,6 +110,15 @@ class AddressableLED { AddressableLED(AddressableLED&&) = default; AddressableLED& operator=(AddressableLED&&) = default; + /** + * Sets the color order for this AddressableLED. The default order is GRB. + * + * This will take effect on the next call to SetData(). + * + * @param order the color order + */ + void SetColorOrder(ColorOrder order); + /** * Sets the length of the LED strip. * @@ -169,4 +193,9 @@ class AddressableLED { hal::Handle m_handle; int m_port; }; + +constexpr auto format_as(AddressableLED::ColorOrder order) { + return static_cast(order); +} + } // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java index a8c160fa4b..613e8edf16 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java @@ -12,13 +12,57 @@ import edu.wpi.first.hal.PWMJNI; /** * A class for driving addressable LEDs, such as WS2812B, WS2815, and NeoPixels. * - *

By default, the timing supports WS2812B and WS2815 LEDs, but is configurable using - * setBitTiming() + *

By default, the timing supports WS2812B and WS2815 LEDs, but is configurable using {@link + * #setBitTiming(int, int, int, int)} + * + *

Some LEDs use a different color order than the default GRB. The color order is configurable + * using {@link #setColorOrder(ColorOrder)}. * *

Only 1 LED driver is currently supported by the roboRIO. However, multiple LED strips can be * connected in series and controlled from the single driver. */ public class AddressableLED implements AutoCloseable { + /** Order that color data is sent over the wire. */ + public enum ColorOrder { + /** RGB order. */ + kRGB(AddressableLEDJNI.COLOR_ORDER_RGB), + /** RBG order. */ + kRBG(AddressableLEDJNI.COLOR_ORDER_RBG), + /** BGR order. */ + kBGR(AddressableLEDJNI.COLOR_ORDER_BGR), + /** BRG order. */ + kBRG(AddressableLEDJNI.COLOR_ORDER_BRG), + /** GBR order. */ + kGBR(AddressableLEDJNI.COLOR_ORDER_GBR), + /** GRB order. This is the default order. */ + kGRB(AddressableLEDJNI.COLOR_ORDER_GRB); + + /** The native value for this ColorOrder. */ + public final int value; + + ColorOrder(int value) { + this.value = value; + } + + /** + * Gets a color order from an int value. + * + * @param value int value + * @return color order + */ + public ColorOrder fromValue(int value) { + return switch (value) { + case AddressableLEDJNI.COLOR_ORDER_RBG -> kRBG; + case AddressableLEDJNI.COLOR_ORDER_BGR -> kBGR; + case AddressableLEDJNI.COLOR_ORDER_BRG -> kBRG; + case AddressableLEDJNI.COLOR_ORDER_GRB -> kGRB; + case AddressableLEDJNI.COLOR_ORDER_GBR -> kGBR; + case AddressableLEDJNI.COLOR_ORDER_RGB -> kRGB; + default -> kGRB; + }; + } + } + private final int m_pwmHandle; private final int m_handle; @@ -43,6 +87,17 @@ public class AddressableLED implements AutoCloseable { } } + /** + * Sets the color order for this AddressableLED. The default order is GRB. + * + *

This will take effect on the next call to {@link #setData(AddressableLEDBuffer)}. + * + * @param order the color order + */ + public void setColorOrder(ColorOrder order) { + AddressableLEDJNI.setColorOrder(m_handle, order.value); + } + /** * Sets the length of the LED strip. *