[hal, wpilib] AddressableLED: allow selecting color order

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.

hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java

@@ -10,6 +10,13 @@
  * @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,14 @@ 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.
+   *
+   * @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.
    *

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

@@ -9,6 +9,7 @@
 
 #include <fmt/format.h>
 
+#include "AddressableLEDSimd.h"
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "FPGACalls.h"
@@ -28,6 +29,7 @@ struct AddressableLED {
   void* ledBuffer;
   size_t ledBufferSize;
   int32_t stringLength = 1;
+  HAL_AddressableLEDColorOrder colorOrder = HAL_ALED_GRB;
 };
 }  // namespace
 
@@ -47,6 +49,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<HAL_ALED_RGB>(reinterpret_cast<const uint8_t*>(src),
+                                  reinterpret_cast<uint8_t*>(dst), len);
+      break;
+    case HAL_ALED_RBG:
+      ConvertPixels<HAL_ALED_RBG>(reinterpret_cast<const uint8_t*>(src),
+                                  reinterpret_cast<uint8_t*>(dst), len);
+      break;
+    case HAL_ALED_BGR:
+      ConvertPixels<HAL_ALED_BGR>(reinterpret_cast<const uint8_t*>(src),
+                                  reinterpret_cast<uint8_t*>(dst), len);
+      break;
+    case HAL_ALED_BRG:
+      ConvertPixels<HAL_ALED_BRG>(reinterpret_cast<const uint8_t*>(src),
+                                  reinterpret_cast<uint8_t*>(dst), len);
+      break;
+    case HAL_ALED_GBR:
+      ConvertPixels<HAL_ALED_GBR>(reinterpret_cast<const uint8_t*>(src),
+                                  reinterpret_cast<uint8_t*>(dst), len);
+      break;
+  }
+}
+
 extern "C" {
 
 HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
@@ -125,6 +158,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 +249,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");