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. *