[wpimath] Fix units typo (#7789)

Signed-off-by: Jade Turner <spacey-sooty@proton.me>

.bazelrc

@@ -16,9 +16,15 @@ import shared/bazel/compiler_flags/base_linux_flags.rc
 import shared/bazel/compiler_flags/linux_flags.rc
 import shared/bazel/compiler_flags/osx_flags.rc
 import shared/bazel/compiler_flags/roborio_flags.rc
+import shared/bazel/compiler_flags/systemcore_flags.rc
 import shared/bazel/compiler_flags/windows_flags.rc
 import shared/bazel/compiler_flags/coverage_flags.rc
 
+# Alias toolchain names to what wpilibsuite uses for CI/Artifact naming
+build:athena --config=roborio
+build:linuxarm32 --config=raspibookworm32
+build:linuxarm64 --config=bookworm64
+
 build:build_java --test_tag_filters=allwpilib-build-java --build_tag_filters=allwpilib-build-java
 build:build_cpp --test_tag_filters=+allwpilib-build-cpp --build_tag_filters=+allwpilib-build-cpp
 build:no_example --test_tag_filters=-wpi-example --build_tag_filters=-wpi-example

.github/labeler.yml

@@ -54,3 +54,12 @@
 'component: wpiutil':
 - changed-files:
   - any-glob-to-any-file: wpiutil/**
+'component: wpical':
+- changed-files:
+  - any-glob-to-any-file: wpical/**
+'component: usage reporting':
+- changed-files:
+  - any-glob-to-any-file: hal/src/generate/**
+'attn: NI':
+- changed-files:
+  - any-glob-to-any-file: hal/src/generate/**

.github/workflows/aql/wpilib-mvn-development_unused.aql

@@ -3,7 +3,7 @@
     {
       "aql": {
         "items.find": {
-          "repo": "wpilib-mvn-development",
+          "repo": "wpilib-mvn-development-local",
           "path": { "$nmatch":"*edu/wpi/first/thirdparty*" },
           "$or":[
             {

.github/workflows/artifactory-nightly-cleanup.yml

@@ -2,6 +2,8 @@ name: Artifactory Nightly Cleanup
 
 on:
   workflow_dispatch:
+  schedule:
+    - cron: '15 2 * * *'
 
 jobs:
   wpilib-mvn-development_unused_cleanup:

.github/workflows/labeler.yml

@@ -1,6 +1,6 @@
 name: "Pull Request Labeler"
 on:
-- pull_request_target
+  - pull_request_target
 
 jobs:
   labeler:
@@ -9,4 +9,6 @@ jobs:
       pull-requests: write
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/labeler@v5
+      - uses: actions/labeler@v5
+        with:
+          sync-labels: true

.github/workflows/upstream-utils.yml

@@ -120,12 +120,6 @@ jobs:
           ./mpack.py clone
           ./mpack.py copy-src
           ./mpack.py format-patch
-      - name: Run stack_walker.py
-        run: |
-          cd upstream_utils
-          ./stack_walker.py clone
-          ./stack_walker.py copy-src
-          ./stack_walker.py format-patch
       - name: Run memory.py
         run: |
           cd upstream_utils

.gitignore

@@ -255,3 +255,6 @@ bazel_auth.rc
 
 # ctest
 /Testing/
+
+# Meson
+.meson-subproject*

ThirdPartyNotices.txt

@@ -33,7 +33,6 @@ jQuery                wpinet/src/main/native/resources/jquery-*
 popper.js             wpinet/src/main/native/resources/popper-*
 units                 wpimath/src/main/native/include/units/
 Eigen                 wpimath/src/main/native/thirdparty/eigen/include/
-StackWalker           wpiutil/src/main/native/windows/StackWalker.*
 Team 254 Library      wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
                       wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
                       wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -54,6 +53,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.
 
@@ -1025,35 +1025,6 @@ Exhibit B - "Incompatible With Secondary Licenses" Notice
   defined by the Mozilla Public License, v. 2.0.
 
 
-===================
-StackWalker License
-===================
-Copyright (c) 2005-2013, Jochen Kalmbach
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice,
-this list of conditions and the following disclaimer.
-Redistributions in binary form must reproduce the above copyright notice,
-this list of conditions and the following disclaimer in the documentation
-and/or other materials provided with the distribution.
-Neither the name of Jochen Kalmbach nor the names of its contributors may be
-used to endorse or promote products derived from this software without
-specific prior written permission.
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
-THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
-FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-
 ================
 Team 254 Library
 ================
@@ -1702,3 +1673,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.

WORKSPACE

@@ -35,8 +35,8 @@ maven_install(
 # Download toolchains
 http_archive(
     name = "rules_bzlmodrio_toolchains",
-    sha256 = "fe267e2af53c1def1e962700a9aeda9e8fdfa9fb46b72167c615ec0e25447dd6",
-    url = "https://github.com/wpilibsuite/rules_bzlmodRio_toolchains/releases/download/2025-1/rules_bzlmodRio_toolchains-2025-1.tar.gz",
+    sha256 = "ff25b5f9445cbd43759be4c6582b987d1065cf817c593eedc7ada1a699298c84",
+    url = "https://github.com/wpilibsuite/rules_bzlmodRio_toolchains/releases/download/2025-1.bcr2/rules_bzlmodRio_toolchains-2025-1.bcr2.tar.gz",
 )
 
 load("@rules_bzlmodrio_toolchains//:maven_deps.bzl", "setup_legacy_setup_toolchains_dependencies")
@@ -50,8 +50,8 @@ load_toolchains()
 #
 http_archive(
     name = "rules_bzlmodrio_jdk",
-    sha256 = "a00d5fa971fbcad8a17b1968cdc5350688397035e90b0cb94e040d375ecd97b4",
-    url = "https://github.com/wpilibsuite/rules_bzlmodRio_jdk/releases/download/17.0.8.1-1/rules_bzlmodRio_jdk-17.0.8.1-1.tar.gz",
+    sha256 = "81869fe9860e39b17e4a9bc1d33c1ca2faede7e31d9538ed0712406f753a2163",
+    url = "https://github.com/wpilibsuite/rules_bzlmodRio_jdk/releases/download/17.0.12-7/rules_bzlmodRio_jdk-17.0.12-7.tar.gz",
 )
 
 load("@rules_bzlmodrio_jdk//:maven_deps.bzl", "setup_legacy_setup_jdk_dependencies")
@@ -62,9 +62,15 @@ register_toolchains(
     "@local_roborio//:macos",
     "@local_roborio//:linux",
     "@local_roborio//:windows",
-    "@local_raspi_32//:macos",
-    "@local_raspi_32//:linux",
-    "@local_raspi_32//:windows",
+    "@local_systemcore//:macos",
+    "@local_systemcore//:linux",
+    "@local_systemcore//:windows",
+    "@local_raspi_bullseye_32//:macos",
+    "@local_raspi_bullseye_32//:linux",
+    "@local_raspi_bullseye_32//:windows",
+    "@local_raspi_bookworm_32//:macos",
+    "@local_raspi_bookworm_32//:linux",
+    "@local_raspi_bookworm_32//:windows",
     "@local_bullseye_32//:macos",
     "@local_bullseye_32//:linux",
     "@local_bullseye_32//:windows",
@@ -83,8 +89,8 @@ setup_legacy_setup_jdk_dependencies()
 
 http_archive(
     name = "bzlmodrio-ni",
-    sha256 = "197fceac88bf44fb8427d5e000b0083118d3346172dd2ad31eccf83a5e61b3ce",
-    url = "https://github.com/wpilibsuite/bzlmodRio-ni/releases/download/2025.0.0/bzlmodRio-ni-2025.0.0.tar.gz",
+    sha256 = "fff62c3cb3e83f9a0d0a01f1739477c9ca5e9a6fac05be1ad59dafcd385801f7",
+    url = "https://github.com/wpilibsuite/bzlmodRio-ni/releases/download/2025.2.0/bzlmodRio-ni-2025.2.0.tar.gz",
 )
 
 load("@bzlmodrio-ni//:maven_cpp_deps.bzl", "setup_legacy_bzlmodrio_ni_cpp_dependencies")
@@ -93,8 +99,8 @@ setup_legacy_bzlmodrio_ni_cpp_dependencies()
 
 http_archive(
     name = "bzlmodrio-opencv",
-    sha256 = "4f4a607956ca8555618736c3058dd96e09d02df19e95088c1e352d2319fd70c7",
-    url = "https://github.com/wpilibsuite/bzlmodRio-opencv/releases/download/2025.4.10.0-2/bzlmodRio-opencv-2025.4.10.0-2.tar.gz",
+    sha256 = "ba3f4910ce9cc0e08abff732aeb5835b1bcfd864ca5296edeadcf2935f7e81b9",
+    url = "https://github.com/wpilibsuite/bzlmodRio-opencv/releases/download/2025.4.10.0-3.bcr1/bzlmodRio-opencv-2025.4.10.0-3.bcr1.tar.gz",
 )
 
 load("@bzlmodrio-opencv//:maven_cpp_deps.bzl", "setup_legacy_bzlmodrio_opencv_cpp_dependencies")

apriltag/convert_apriltag_layouts.py

@@ -7,10 +7,11 @@ AprilTagFields expects.
 
 The input CSV has the following format:
 
-* Columns: ID, X, Y, Z, Rotation
+* Columns: ID, X, Y, Z, Z Rotation, Y Rotation
 * ID is a positive integer
 * X, Y, and Z are decimal inches
-* Rotation is yaw in degrees
+* Z Rotation is yaw in degrees
+* Y Rotation is pitch in degrees
 
 The values come from a table in the layout marking diagram (e.g.,
 https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf).
@@ -48,13 +49,14 @@ def main():
                 x = float(row[1])
                 y = float(row[2])
                 z = float(row[3])
-                rotation = float(row[4])
+                zRotation = float(row[4])
+                yRotation = float(row[5])
 
                 # Turn yaw into quaternion
                 q = geometry.Rotation3d(
-                    units.radians(0.0),
-                    units.radians(0.0),
-                    units.degreesToRadians(rotation),
+                    units.radians(0),
+                    units.degreesToRadians(yRotation),
+                    units.degreesToRadians(zRotation),
                 ).getQuaternion()
 
                 json_data["tags"].append(

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -13,13 +13,17 @@ public enum AprilTagFields {
   /** 2023 Charged Up. */
   k2023ChargedUp("2023-chargedup.json"),
   /** 2024 Crescendo. */
-  k2024Crescendo("2024-crescendo.json");
+  k2024Crescendo("2024-crescendo.json"),
+  /** 2025 Reefscape Welded (see TU 12). */
+  k2025ReefscapeWelded("2025-reefscape-welded.json"),
+  /** 2025 Reefscape AndyMark (see TU 12). */
+  k2025ReefscapeAndyMark("2025-reefscape-andymark.json");
 
   /** Base resource directory. */
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2024Crescendo;
+  public static final AprilTagFields kDefaultField = k2025ReefscapeWelded;
 
   /** Resource filename. */
   public final String m_resourceFile;

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -133,6 +133,8 @@ namespace frc {
 std::string_view GetResource_2022_rapidreact_json();
 std::string_view GetResource_2023_chargedup_json();
 std::string_view GetResource_2024_crescendo_json();
+std::string_view GetResource_2025_reefscape_welded_json();
+std::string_view GetResource_2025_reefscape_andymark_json();
 
 }  // namespace frc
 
@@ -148,6 +150,12 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
     case AprilTagField::k2024Crescendo:
       fieldString = GetResource_2024_crescendo_json();
       break;
+    case AprilTagField::k2025ReefscapeWelded:
+      fieldString = GetResource_2025_reefscape_welded_json();
+      break;
+    case AprilTagField::k2025ReefscapeAndyMark:
+      fieldString = GetResource_2025_reefscape_andymark_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -20,8 +20,12 @@ enum class AprilTagField {
   k2023ChargedUp,
   /// 2024 Crescendo.
   k2024Crescendo,
+  /// 2025 Reefscape AndyMark (see TU12).
+  k2025ReefscapeAndyMark,
+  /// 2025 Reefscape Welded (see TU12).
+  k2025ReefscapeWelded,
   /// Alias to the current game.
-  kDefaultField = k2024Crescendo,
+  kDefaultField = k2025ReefscapeWelded,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv

@@ -1,17 +0,0 @@
-ID,X,Y,Z,Rotation
-1,593.68,9.68,53.38,120
-2,637.21,34.79,53.38,120
-3,652.73,196.17,57.13,180
-4,652.73,218.42,57.13,180
-5,578.77,323.00,53.38,270
-6,72.5,323.00,53.38,270
-7,-1.50,218.42,57.13,0
-8,-1.50,196.17,57.13,0
-9,14.02,34.79,53.38,60
-10,57.54,9.68,53.38,60
-11,468.69,146.19,52.00,300
-12,468.69,177.10,52.00,60
-13,441.74,161.62,52.00,180
-14,209.48,161.62,52.00,0
-15,182.73,177.10,52.00,120
-16,182.73,146.19,52.00,240

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.csv

@@ -0,0 +1,23 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,656.98,24.73,58.5,126,0
+2,656.98,291.9,58.5,234,0
+3,452.4,316.21,51.25,270,0
+4,365.2,241.44,73.54,0,30
+5,365.2,75.19,73.54,0,30
+6,530.49,129.97,12.13,300,0
+7,546.87,158.3,12.13,0,0
+8,530.49,186.63,12.13,60,0
+9,497.77,186.63,12.13,120,0
+10,481.39,158.3,12.13,180,0
+11,497.77,129.97,12.13,240,0
+12,33.91,24.73,58.5,54,0
+13,33.91,291.9,58.5,306,0
+14,325.68,241.44,73.54,180,30
+15,325.68,75.19,73.54,180,30
+16,238.49,0.42,51.25,90,0
+17,160.39,129.97,12.13,240,0
+18,144,158.3,12.13,180,0
+19,160.39,186.63,12.13,120,0
+20,193.1,186.63,12.13,60,0
+21,209.49,158.3,12.13,0,0
+22,193.1,129.97,12.13,300,0

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.json

@@ -0,0 +1,404 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 16.687292,
+          "y": 0.628142,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.4539904997395468,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883678
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.687292,
+          "y": 7.414259999999999,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.45399049973954675,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883679
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.49096,
+          "y": 8.031733999999998,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 6.132575999999999,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 1.9098259999999998,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 13.890498,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.227305999999999,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 0.8613139999999999,
+          "y": 0.628142,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8910065241883679,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954675
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 0.8613139999999999,
+          "y": 7.414259999999999,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8910065241883678,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954686
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 6.132575999999999,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 1.9098259999999998,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 6.057646,
+          "y": 0.010667999999999999,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 3.6576,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 5.321046,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 17.548,
+    "width": 8.042
+  }
+}

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.csv

@@ -0,0 +1,23 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,657.37,25.8,58.5,126,0
+2,657.37,291.2,58.5,234,0
+3,455.15,317.15,51.25,270,0
+4,365.2,241.64,73.54,0,30
+5,365.2,75.39,73.54,0,30
+6,530.49,130.17,12.13,300,0
+7,546.87,158.5,12.13,0,0
+8,530.49,186.83,12.13,60,0
+9,497.77,186.83,12.13,120,0
+10,481.39,158.5,12.13,180,0
+11,497.77,130.17,12.13,240,0
+12,33.51,25.8,58.5,54,0
+13,33.51,291.2,58.5,306,0
+14,325.68,241.64,73.54,180,30
+15,325.68,75.39,73.54,180,30
+16,235.73,-0.15,51.25,90,0
+17,160.39,130.17,12.13,240,0
+18,144,158.5,12.13,180,0
+19,160.39,186.83,12.13,120,0
+20,193.1,186.83,12.13,60,0
+21,209.49,158.5,12.13,0,0
+22,193.1,130.17,12.13,300,0

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.json

@@ -0,0 +1,404 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 16.697198,
+          "y": 0.65532,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.4539904997395468,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883678
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.697198,
+          "y": 7.3964799999999995,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.45399049973954675,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883679
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.560809999999998,
+          "y": 8.05561,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 6.137656,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 1.914906,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 13.890498,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.227305999999999,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 0.851154,
+          "y": 0.65532,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8910065241883679,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954675
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 0.851154,
+          "y": 7.3964799999999995,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8910065241883678,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954686
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 6.137656,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 1.914906,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 5.9875419999999995,
+          "y": -0.0038099999999999996,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 3.6576,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 4.745482,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 5.321046,
+          "y": 4.0259,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 3.3063179999999996,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 17.548,
+    "width": 8.052
+  }
+}

buildSrc/build.gradle

@@ -9,5 +9,5 @@ repositories {
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2025.9.0"
+    implementation "edu.wpi.first:native-utils:2025.9.1"
 }

buildSrc/src/main/groovy/WPIJREArtifact.groovy

@@ -28,7 +28,7 @@ public class WPIJREArtifact extends MavenArtifact {
 
     private boolean checkJreVersion = true;
 
-    private final String artifactLocation = "edu.wpi.first.jdk:roborio-2024:17.0.9u7-1"
+    private final String artifactLocation = "edu.wpi.first.jdk:roborio-2024:17.0.9u7-3"
 
     @Inject
     public WPIJREArtifact(String name, RemoteTarget target) {

cmake/modules/CompileWarnings.cmake

@@ -46,7 +46,7 @@ macro(wpilib_target_warnings target)
 
     # Suppress warning "enumeration types with a fixed underlying type are a
     # Clang extension"
-    if(APPLE)
+    if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
         target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:C>:-Wno-fixed-enum-extension>)
     endif()
 

cscore/src/main/java/edu/wpi/first/cscore/CvSink.java

@@ -6,6 +6,7 @@ package edu.wpi.first.cscore;
 
 import edu.wpi.first.util.PixelFormat;
 import edu.wpi.first.util.RawFrame;
+import edu.wpi.first.util.TimestampSource;
 import java.nio.ByteBuffer;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
@@ -220,4 +221,22 @@ public class CvSink extends ImageSink {
     }
     return rv;
   }
+
+  /**
+   * Get the last time a frame was grabbed. This uses the same time base as wpi::Now().
+   *
+   * @return Time in 1 us increments.
+   */
+  public long getLastFrameTime() {
+    return m_frame.getTimestamp();
+  }
+
+  /**
+   * Get the time source for the timestamp the last frame was grabbed at.
+   *
+   * @return Time source
+   */
+  public TimestampSource getLastFrameTimeSource() {
+    return m_frame.getTimestampSource();
+  }
 }

cscore/src/main/native/cpp/Frame.cpp

@@ -16,18 +16,22 @@
 
 using namespace cs;
 
-Frame::Frame(SourceImpl& source, std::string_view error, Time time)
+Frame::Frame(SourceImpl& source, std::string_view error, Time time,
+             WPI_TimestampSource timeSrc)
     : m_impl{source.AllocFrameImpl().release()} {
   m_impl->refcount = 1;
   m_impl->error = error;
   m_impl->time = time;
+  m_impl->timeSource = timeSrc;
 }
 
-Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time)
+Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
+             WPI_TimestampSource timeSrc)
     : m_impl{source.AllocFrameImpl().release()} {
   m_impl->refcount = 1;
   m_impl->error.resize(0);
   m_impl->time = time;
+  m_impl->timeSource = timeSrc;
   m_impl->images.push_back(image.release());
 }
 

cscore/src/main/native/cpp/Frame.h

@@ -39,6 +39,7 @@ class Frame {
     wpi::recursive_mutex mutex;
     std::atomic_int refcount{0};
     Time time{0};
+    WPI_TimestampSource timeSource{WPI_TIMESRC_UNKNOWN};
     SourceImpl& source;
     std::string error;
     wpi::SmallVector<Image*, 4> images;
@@ -48,9 +49,11 @@ class Frame {
  public:
   Frame() noexcept = default;
 
-  Frame(SourceImpl& source, std::string_view error, Time time);
+  Frame(SourceImpl& source, std::string_view error, Time time,
+        WPI_TimestampSource timeSrc);
 
-  Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time);
+  Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
+        WPI_TimestampSource timeSrc);
 
   Frame(const Frame& frame) noexcept : m_impl{frame.m_impl} {
     if (m_impl) {
@@ -75,6 +78,9 @@ class Frame {
   }
 
   Time GetTime() const { return m_impl ? m_impl->time : 0; }
+  WPI_TimestampSource GetTimeSource() const {
+    return m_impl ? m_impl->timeSource : WPI_TIMESRC_UNKNOWN;
+  }
 
   std::string_view GetError() const {
     if (!m_impl) {

cscore/src/main/native/cpp/RawSinkImpl.cpp

@@ -120,6 +120,8 @@ uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame,
   rawFrame.pixelFormat = newImage->pixelFormat;
   rawFrame.size = newImage->size();
   std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data);
+  rawFrame.timestamp = incomingFrame.GetTime();
+  rawFrame.timestampSrc = incomingFrame.GetTimeSource();
 
   return incomingFrame.GetTime();
 }

cscore/src/main/native/cpp/SourceImpl.cpp

@@ -29,7 +29,7 @@ SourceImpl::SourceImpl(std::string_view name, wpi::Logger& logger,
       m_notifier(notifier),
       m_telemetry(telemetry),
       m_name{name} {
-  m_frame = Frame{*this, std::string_view{}, 0};
+  m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
 }
 
 SourceImpl::~SourceImpl() {
@@ -95,7 +95,8 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
   if (!m_frameCv.wait_for(
           lock, std::chrono::milliseconds(static_cast<int>(timeout * 1000)),
           [=, this] { return m_frame.GetTime() != lastFrameTime; })) {
-    m_frame = Frame{*this, "timed out getting frame", wpi::Now()};
+    m_frame = Frame{*this, "timed out getting frame", wpi::Now(),
+                    WPI_TIMESRC_UNKNOWN};
   }
   return m_frame;
 }
@@ -103,7 +104,7 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
 void SourceImpl::Wakeup() {
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, std::string_view{}, 0};
+    m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
   }
   m_frameCv.notify_all();
 }
@@ -463,7 +464,8 @@ std::unique_ptr<Image> SourceImpl::AllocImage(
 }
 
 void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
-                          int height, std::string_view data, Frame::Time time) {
+                          int height, std::string_view data, Frame::Time time,
+                          WPI_TimestampSource timeSrc) {
   if (pixelFormat == VideoMode::PixelFormat::kBGRA) {
     // Write BGRA as BGR to save a copy
     auto image =
@@ -480,10 +482,11 @@ void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
           fmt::ptr(data.data()), data.size());
   std::memcpy(image->data(), data.data(), data.size());
 
-  PutFrame(std::move(image), time);
+  PutFrame(std::move(image), time, timeSrc);
 }
 
-void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
+void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time,
+                          WPI_TimestampSource timeSrc) {
   // Update telemetry
   m_telemetry.RecordSourceFrames(*this, 1);
   m_telemetry.RecordSourceBytes(*this, static_cast<int>(image->size()));
@@ -491,7 +494,7 @@ void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
   // Update frame
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, std::move(image), time};
+    m_frame = Frame{*this, std::move(image), time, timeSrc};
   }
 
   // Signal listeners
@@ -502,7 +505,7 @@ void SourceImpl::PutError(std::string_view msg, Frame::Time time) {
   // Update frame
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, msg, time};
+    m_frame = Frame{*this, msg, time, WPI_TIMESRC_UNKNOWN};
   }
 
   // Signal listeners

cscore/src/main/native/cpp/SourceImpl.h

@@ -13,6 +13,7 @@
 #include <vector>
 
 #include <wpi/Logger.h>
+#include <wpi/RawFrame.h>
 #include <wpi/condition_variable.h>
 #include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
@@ -141,8 +142,10 @@ class SourceImpl : public PropertyContainer {
                            std::string_view valueStr) override;
 
   void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height,
-                std::string_view data, Frame::Time time);
-  void PutFrame(std::unique_ptr<Image> image, Frame::Time time);
+                std::string_view data, Frame::Time time,
+                WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
+  void PutFrame(std::unique_ptr<Image> image, Frame::Time time,
+                WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
   void PutError(std::string_view msg, Frame::Time time);
 
   // Notification functions for corresponding atomics

cscore/src/main/native/include/cscore_cv.h

@@ -8,6 +8,7 @@
 #include <functional>
 
 #include <opencv2/core/mat.hpp>
+#include <wpi/RawFrame.h>
 
 #include "cscore_oo.h"
 #include "cscore_raw.h"
@@ -172,6 +173,23 @@ class CvSink : public ImageSink {
   uint64_t GrabFrameDirectLastTime(cv::Mat& image, uint64_t lastFrameTime,
                                    double timeout = 0.225);
 
+  /**
+   * Get the last time a frame was grabbed. This uses the same time base as
+   * wpi::Now().
+   *
+   * @return Time in 1 us increments.
+   */
+  [[nodiscard]]
+  uint64_t LastFrameTime();
+
+  /**
+   * Get the time source for the timestamp the last frame was grabbed at.
+   *
+   * @return Time source
+   */
+  [[nodiscard]]
+  WPI_TimestampSource LastFrameTimeSource();
+
  private:
   constexpr int GetCvFormat(WPI_PixelFormat pixelFormat);
 
@@ -405,6 +423,14 @@ inline uint64_t CvSink::GrabFrameDirectLastTime(cv::Mat& image,
   return timestamp;
 }
 
+inline uint64_t CvSink::LastFrameTime() {
+  return rawFrame.timestamp;
+}
+
+inline WPI_TimestampSource CvSink::LastFrameTimeSource() {
+  return static_cast<WPI_TimestampSource>(rawFrame.timestampSrc);
+}
+
 }  // namespace cs
 
 #endif  // CSCORE_CSCORE_CV_H_

cscore/src/main/native/linux/UsbCameraImpl.cpp

@@ -555,8 +555,51 @@ void UsbCameraImpl::CameraThreadMain() {
           good = false;
         }
         if (good) {
+          Frame::Time frameTime{wpi::Now()};
+          WPI_TimestampSource timeSource{WPI_TIMESRC_FRAME_DEQUEUE};
+
+          // check the timestamp time
+          auto tsFlags = buf.flags & V4L2_BUF_FLAG_TIMESTAMP_MASK;
+          SDEBUG4("Flags {}", tsFlags);
+          if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN) {
+            SDEBUG4("Got unknown time for frame - default to wpi::Now");
+          } else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC) {
+            SDEBUG4("Got valid monotonic time for frame");
+            // we can't go directly to frametime, since the rest of cscore
+            // expects us to use wpi::Now, which is in an arbitrary timebase
+            // (see timestamp.cpp). Best I can do is (approximately) translate
+            // between timebases
+
+            // grab current time in the same timebase as buf.timestamp
+            struct timespec ts;
+            if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) {
+              int64_t nowTime = {ts.tv_sec * 1'000'000 + ts.tv_nsec / 1000};
+              int64_t bufTime = {buf.timestamp.tv_sec * 1'000'000 +
+                                 buf.timestamp.tv_usec};
+              // And offset frameTime by the latency
+              int64_t offset{nowTime - bufTime};
+              frameTime -= offset;
+
+              // Figure out the timestamp's source
+              int tsrcFlags = buf.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+              if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_EOF) {
+                timeSource = WPI_TIMESRC_V4L_EOF;
+              } else if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_SOE) {
+                timeSource = WPI_TIMESRC_V4L_SOE;
+              } else {
+                timeSource = WPI_TIMESRC_UNKNOWN;
+              }
+              SDEBUG4("Frame was {} uS old, flags {}, source {}", offset,
+                      tsrcFlags, static_cast<int>(timeSource));
+            } else {
+              // Can't do anything if we can't access the clock, leave default
+            }
+          } else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_COPY) {
+            SDEBUG4("Got valid copy time for frame - default to wpi::Now");
+          }
+
           PutFrame(static_cast<VideoMode::PixelFormat>(m_mode.pixelFormat),
-                   width, height, image, wpi::Now());  // TODO: time
+                   width, height, image, frameTime, timeSource);
         }
       }
 

epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java

@@ -68,7 +68,9 @@ public class AnnotationProcessor extends AbstractProcessor {
               customLoggers.putAll(processCustomLoggers(roundEnv, customLogger));
             });
 
+    // Get all root types (classes and interfaces), excluding packages and modules
     roundEnv.getRootElements().stream()
+        .filter(e -> e instanceof TypeElement)
         .filter(
             e ->
                 processingEnv

epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java

@@ -1974,6 +1974,37 @@ class AnnotationProcessorTest {
     assertLoggerGenerates(source, expectedRootLogger);
   }
 
+  @Test
+  void doesNotBreakWithPackageInfo() {
+    String source =
+        """
+        package example;
+
+        import edu.wpi.first.epilogue.*;
+
+        @Logged
+        class Example {}
+        """;
+
+    String packageInfo = """
+        package example;
+        """;
+
+    Compilation compilation =
+        javac()
+            .withOptions(kJavaVersionOptions)
+            .withProcessors(new AnnotationProcessor())
+            .compile(
+                JavaFileObjects.forSourceString("example.Example", source),
+                JavaFileObjects.forSourceString("example.package-info", packageInfo));
+
+    assertThat(compilation).succeeded();
+    compilation.generatedSourceFiles().stream()
+        .filter(jfo -> jfo.getName().contains("Example"))
+        .findFirst()
+        .orElseThrow(() -> new IllegalStateException("Logger file was not generated!"));
+  }
+
   private void assertCompilationError(
       String message, long lineNumber, long col, Diagnostic<? extends JavaFileObject> diagnostic) {
     assertAll(

epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java

@@ -198,10 +198,9 @@ public interface EpilogueBackend {
    *
    * @param identifier the identifier of the data field
    * @param value the new value of the data field
-   * @param <U> the dimension of the unit
    */
-  default <U extends Unit> void log(String identifier, Measure<U> value) {
-    log(identifier, value, value.baseUnit());
+  default void log(String identifier, Measure<?> value) {
+    log(identifier, value.baseUnitMagnitude());
   }
 
   /**
@@ -213,7 +212,7 @@ public interface EpilogueBackend {
    * @param <U> the dimension of the unit
    */
   default <U extends Unit> void log(String identifier, Measure<U> value, U unit) {
-    log(identifier + " (" + unit.symbol() + ")", value.in(unit));
+    log(identifier, value.in(unit));
   }
 
   /**

epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java

@@ -117,7 +117,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -130,7 +130,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -143,7 +143,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -156,7 +156,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -169,7 +169,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -182,7 +182,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -208,7 +208,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value);
   }
 
@@ -234,7 +234,7 @@ public class LazyBackend implements EpilogueBackend {
       return;
     }
 
-    m_previousValues.put(identifier, value);
+    m_previousValues.put(identifier, value.clone());
     m_backend.log(identifier, value, struct);
   }
 }

epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/CustomStruct.java

@@ -0,0 +1,45 @@
+// 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.
+
+package edu.wpi.first.epilogue.logging;
+
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.nio.ByteBuffer;
+
+public record CustomStruct(int x) implements StructSerializable {
+  public static final Serializer struct = new Serializer();
+
+  public static final class Serializer implements Struct<CustomStruct> {
+    @Override
+    public Class<CustomStruct> getTypeClass() {
+      return CustomStruct.class;
+    }
+
+    @Override
+    public String getTypeName() {
+      return "CustomStruct";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeInt32;
+    }
+
+    @Override
+    public String getSchema() {
+      return "int32 x;";
+    }
+
+    @Override
+    public CustomStruct unpack(ByteBuffer bb) {
+      return new CustomStruct(bb.getInt());
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, CustomStruct value) {
+      bb.putInt(value.x);
+    }
+  }
+}

epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.epilogue.logging;
 
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertSame;
 
@@ -53,4 +54,135 @@ class LazyBackendTest {
           backend.getEntries());
     }
   }
+
+  @Test
+  void inPlaceByteArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    byte[] arr = new byte[] {0};
+    lazy.log("arr", arr);
+
+    arr[0] = 1;
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new byte[] {0}, (byte[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new byte[] {1}, (byte[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceIntArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    int[] arr = new int[] {0};
+    lazy.log("arr", arr);
+
+    arr[0] = 1;
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new int[] {0}, (int[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new int[] {1}, (int[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceLongArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    long[] arr = new long[] {0};
+    lazy.log("arr", arr);
+
+    arr[0] = 1;
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new long[] {0}, (long[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new long[] {1}, (long[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceFloatArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    float[] arr = new float[] {0};
+    lazy.log("arr", arr);
+
+    arr[0] = 1;
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new float[] {0}, (float[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new float[] {1}, (float[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceDoubleArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    double[] arr = new double[] {0};
+    lazy.log("arr", arr);
+
+    arr[0] = 1;
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new double[] {0}, (double[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new double[] {1}, (double[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceBooleanArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    boolean[] arr = new boolean[] {false};
+    lazy.log("arr", arr);
+
+    arr[0] = true;
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new boolean[] {false}, (boolean[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new boolean[] {true}, (boolean[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceStringArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    String[] arr = new String[] {"0"};
+    lazy.log("arr", arr);
+
+    arr[0] = "1";
+    lazy.log("arr", arr);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(new String[] {"0"}, (String[]) backend.getEntries().get(0).value());
+    assertArrayEquals(new String[] {"1"}, (String[]) backend.getEntries().get(1).value());
+  }
+
+  @Test
+  void inPlaceStructArray() {
+    var backend = new TestBackend();
+    var lazy = new LazyBackend(backend);
+
+    CustomStruct[] arr = new CustomStruct[] {new CustomStruct(0)};
+
+    lazy.log("arr", arr, CustomStruct.struct);
+
+    arr[0] = new CustomStruct(1);
+    lazy.log("arr", arr, CustomStruct.struct);
+
+    assertEquals(2, backend.getEntries().size());
+    assertArrayEquals(
+        new byte[] {0x00, 0x00, 0x00, 0x00}, (byte[]) backend.getEntries().get(0).value());
+    assertArrayEquals(
+        new byte[] {0x01, 0x00, 0x00, 0x00}, (byte[]) backend.getEntries().get(1).value());
+  }
 }

epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java

@@ -55,32 +55,32 @@ public class TestBackend implements EpilogueBackend {
 
   @Override
   public void log(String identifier, byte[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
   public void log(String identifier, int[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
   public void log(String identifier, long[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
   public void log(String identifier, float[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
   public void log(String identifier, double[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
   public void log(String identifier, boolean[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
@@ -90,19 +90,27 @@ public class TestBackend implements EpilogueBackend {
 
   @Override
   public void log(String identifier, String[] value) {
-    m_entries.add(new LogEntry<>(identifier, value));
+    m_entries.add(new LogEntry<>(identifier, value.clone()));
   }
 
   @Override
   public <S> void log(String identifier, S value, Struct<S> struct) {
-    var serialized = StructBuffer.create(struct).write(value).array();
+    var buffer = StructBuffer.create(struct).write(value).position(0);
+    var serialized = new byte[buffer.capacity()];
+    for (int i = 0; i < buffer.capacity(); i++) {
+      serialized[i] = buffer.get();
+    }
 
     m_entries.add(new LogEntry<>(identifier, serialized));
   }
 
   @Override
   public <S> void log(String identifier, S[] value, Struct<S> struct) {
-    var serialized = StructBuffer.create(struct).writeArray(value).array();
+    var buffer = StructBuffer.create(struct).writeArray(value).position(0);
+    var serialized = new byte[buffer.capacity()];
+    for (int i = 0; i < buffer.capacity(); i++) {
+      serialized[i] = buffer.get();
+    }
 
     m_entries.add(new LogEntry<>(identifier, serialized));
   }

fieldImages/CMakeLists.txt

@@ -36,13 +36,13 @@ endif()
 
 generate_resources(
     src/main/native/resources/edu/wpi/first/fields
-    generated/main/cpp
+    ${CMAKE_CURRENT_BINARY_DIR}/generated/main/cpp
     FIELDS
     fields
     field_images_resources_src
 )
 
-add_library(fieldImages src/main/native/cpp/fields.cpp ${field_images_resources_src})
+add_library(fieldImages ${field_images_resources_src} src/main/native/cpp/fields.cpp)
 set_target_properties(fieldImages PROPERTIES DEBUG_POSTFIX "d")
 
 set_property(TARGET fieldImages PROPERTY FOLDER "libraries")

fieldImages/src/main/java/edu/wpi/first/fields/Fields.java

@@ -16,12 +16,13 @@ public enum Fields {
   k2021Slalom("2021-slalompath.json"),
   k2022RapidReact("2022-rapidreact.json"),
   k2023ChargedUp("2023-chargedup.json"),
-  k2024Crescendo("2024-crescendo.json");
+  k2024Crescendo("2024-crescendo.json"),
+  k2025Reefscape("2025-reefscape.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2024Crescendo;
+  public static final Fields kDefaultField = k2025Reefscape;
 
   public final String m_resourceFile;
 

fieldImages/src/main/native/cpp/fields.cpp

@@ -16,10 +16,13 @@
 #include "fields/2022-rapidreact.h"
 #include "fields/2023-chargedup.h"
 #include "fields/2024-crescendo.h"
+#include "fields/2025-reefscape.h"
 
 using namespace fields;
 
 static const Field kFields[] = {
+    {"2025 Reefscape", GetResource_2025_reefscape_json,
+     GetResource_2025_field_png},
     {"2024 Crescendo", GetResource_2024_crescendo_json,
      GetResource_2024_field_png},
     {"2023 Charged Up", GetResource_2023_chargedup_json,

fieldImages/src/main/native/include/fields/2025-reefscape.h

@@ -0,0 +1,12 @@
+// 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 <string_view>
+
+namespace fields {
+std::string_view GetResource_2025_reefscape_json();
+std::string_view GetResource_2025_field_png();
+}  // namespace fields

fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json

@@ -0,0 +1,19 @@
+{
+  "game": "Reefscape",
+  "field-image": "2025-field.png",
+  "field-corners": {
+    "top-left": [
+      534,
+      291
+    ],
+    "bottom-right": [
+      3466,
+      1638
+    ]
+  },
+  "field-size": [
+    57.573,
+    26.417
+  ],
+  "field-unit": "foot"
+}

glass/src/lib/native/cpp/other/Field2D.cpp

@@ -373,13 +373,12 @@ void FieldInfo::DisplaySettings() {
     }
     ImGui::EndCombo();
   }
-  if (m_builtin.empty() && ImGui::Button("Load image...")) {
+  if (m_builtin.empty() && ImGui::Button("Load JSON/image...")) {
     m_fileOpener = std::make_unique<pfd::open_file>(
-        "Choose field image", "",
-        std::vector<std::string>{"Image File",
+        "Choose field JSON/image", "",
+        std::vector<std::string>{"PathWeaver JSON File", "*.json", "Image File",
                                  "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
-                                 "*.hdr *.pic *.ppm *.pgm",
-                                 "PathWeaver JSON File", "*.json"});
+                                 "*.hdr *.pic *.ppm *.pgm"});
   }
   if (ImGui::Button("Reset image")) {
     Reset();
@@ -586,17 +585,29 @@ FieldFrameData FieldInfo::GetFrameData(ImVec2 min, ImVec2 max) const {
     max.x -= (m_imageWidth - m_right) * scale;
     max.y -= (m_imageHeight - m_bottom) * scale;
   } else if ((max.x - min.x) > 40 && (max.y - min.y > 40)) {
+    // scale padding to be proportional to aspect ratio
+    float width = max.x - min.x;
+    float height = max.y - min.y;
+    float padX, padY;
+    if (width > height) {
+      padX = 20 * width / height;
+      padY = 20;
+    } else {
+      padX = 20;
+      padY = 20 * height / width;
+    }
+
     // ensure there's some padding
-    min.x += 20;
-    max.x -= 20;
-    min.y += 20;
-    max.y -= 20;
+    min.x += padX;
+    max.x -= padX;
+    min.y += padY;
+    max.y -= padY;
 
     // also pad the image so it's the same size as the box
-    ffd.imageMin.x += 20;
-    ffd.imageMax.x -= 20;
-    ffd.imageMin.y += 20;
-    ffd.imageMax.y -= 20;
+    ffd.imageMin.x += padX;
+    ffd.imageMax.x -= padX;
+    ffd.imageMin.y += padY;
+    ffd.imageMax.y -= padY;
   }
 
   ffd.min = min;

hal/src/generate/Instances.txt

@@ -15,6 +15,8 @@ kFramework_ROS = 5
 kFramework_RobotBuilder = 6
 kFramework_AdvantageKit = 7
 kFramework_MagicBot = 8
+kFramework_KitBotTraditional = 9
+kFramework_KitBotInline = 10
 kRobotDrive_ArcadeStandard = 1
 kRobotDrive_ArcadeButtonSpin = 2
 kRobotDrive_ArcadeRatioCurve = 3

hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java

@@ -317,6 +317,10 @@ public final class FRCNetComm {
     public static final int kFramework_AdvantageKit = 7;
     /** kFramework_MagicBot = 8. */
     public static final int kFramework_MagicBot = 8;
+    /** kFramework_KitBotTraditional = 9. */
+    public static final int kFramework_KitBotTraditional = 9;
+    /** kFramework_KitBotInline = 10. */
+    public static final int kFramework_KitBotInline = 10;
     /** kRobotDrive_ArcadeStandard = 1. */
     public static final int kRobotDrive_ArcadeStandard = 1;
     /** kRobotDrive_ArcadeButtonSpin = 2. */

hal/src/generated/main/native/include/hal/FRCUsageReporting.h

@@ -197,6 +197,8 @@ namespace HALUsageReporting {
     kFramework_RobotBuilder = 6,
     kFramework_AdvantageKit = 7,
     kFramework_MagicBot = 8,
+    kFramework_KitBotTraditional = 9,
+    kFramework_KitBotInline = 10,
     kRobotDrive_ArcadeStandard = 1,
     kRobotDrive_ArcadeButtonSpin = 2,
     kRobotDrive_ArcadeRatioCurve = 3,

hal/src/generated/main/native/include/hal/UsageReporting.h

@@ -170,6 +170,8 @@ typedef enum
     kFramework_RobotBuilder = 6,
     kFramework_AdvantageKit = 7,
     kFramework_MagicBot = 8,
+    kFramework_KitBotTraditional = 9,
+    kFramework_KitBotInline = 10,
     kRobotDrive_ArcadeStandard = 1,
     kRobotDrive_ArcadeButtonSpin = 2,
     kRobotDrive_ArcadeRatioCurve = 3,

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.
+   *
+   * <p>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.
    *
@@ -53,7 +70,8 @@ public class AddressableLEDJNI extends JNIWrapper {
   /**
    * Sets the bit timing.
    *
-   * <p>By default, the driver is set up to drive WS2812Bs, so nothing needs to be set for those.
+   * <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing needs to be set for
+   * those.
    *
    * @param handle the Addressable LED handle
    * @param highTime0NanoSeconds high time for 0 bit (default 400ns)
@@ -72,7 +90,7 @@ public class AddressableLEDJNI extends JNIWrapper {
   /**
    * Sets the sync time.
    *
-   * <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B.
+   * <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815.
    *
    * @param handle the Addressable LED handle
    * @param syncTimeMicroSeconds the sync time (default 280us)

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

@@ -61,12 +61,12 @@ public class SPIJNI extends JNIWrapper {
    * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
    * @param dataToSend Buffer of data to send as part of the transaction.
    * @param dataReceived Buffer to read data into.
-   * @param size Number of bytes to transfer. [0..7]
+   * @param size Number of bytes to transfer.
    * @return Number of bytes transferred, -1 for error
    * @see "HAL_TransactionSPI"
    */
   public static native int spiTransaction(
-      int port, ByteBuffer dataToSend, ByteBuffer dataReceived, byte size);
+      int port, ByteBuffer dataToSend, ByteBuffer dataReceived, int size);
 
   /**
    * Performs an SPI send/receive transaction.
@@ -77,12 +77,12 @@ public class SPIJNI extends JNIWrapper {
    * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
    * @param dataToSend Buffer of data to send as part of the transaction.
    * @param dataReceived Buffer to read data into.
-   * @param size Number of bytes to transfer. [0..7]
+   * @param size Number of bytes to transfer.
    * @return Number of bytes transferred, -1 for error
    * @see "HAL_TransactionSPI"
    */
   public static native int spiTransactionB(
-      int port, byte[] dataToSend, byte[] dataReceived, byte size);
+      int port, byte[] dataToSend, byte[] dataReceived, int size);
 
   /**
    * Executes a write transaction with the device.
@@ -95,7 +95,7 @@ public class SPIJNI extends JNIWrapper {
    * @return The number of bytes written. -1 for an error
    * @see "HAL_WriteSPI"
    */
-  public static native int spiWrite(int port, ByteBuffer dataToSend, byte sendSize);
+  public static native int spiWrite(int port, ByteBuffer dataToSend, int sendSize);
 
   /**
    * Executes a write transaction with the device.
@@ -108,7 +108,7 @@ public class SPIJNI extends JNIWrapper {
    * @return The number of bytes written. -1 for an error
    * @see "HAL_WriteSPI"
    */
-  public static native int spiWriteB(int port, byte[] dataToSend, byte sendSize);
+  public static native int spiWriteB(int port, byte[] dataToSend, int sendSize);
 
   /**
    * Executes a read from the device.
@@ -121,11 +121,11 @@ public class SPIJNI extends JNIWrapper {
    * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
    * @param initiate initiates a transaction when true. Just reads when false.
    * @param dataReceived A pointer to the array of bytes to store the data read from the device.
-   * @param size The number of bytes to read in the transaction. [1..7]
+   * @param size The number of bytes to read in the transaction.
    * @return Number of bytes read. -1 for error.
    * @see "HAL_ReadSPI"
    */
-  public static native int spiRead(int port, boolean initiate, ByteBuffer dataReceived, byte size);
+  public static native int spiRead(int port, boolean initiate, ByteBuffer dataReceived, int size);
 
   /**
    * Executes a read from the device.
@@ -138,11 +138,11 @@ public class SPIJNI extends JNIWrapper {
    * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
    * @param initiate initiates a transaction when true. Just reads when false.
    * @param dataReceived A pointer to the array of bytes to store the data read from the device.
-   * @param size The number of bytes to read in the transaction. [1..7]
+   * @param size The number of bytes to read in the transaction.
    * @return Number of bytes read. -1 for error.
    * @see "HAL_ReadSPI"
    */
-  public static native int spiReadB(int port, boolean initiate, byte[] dataReceived, byte size);
+  public static native int spiReadB(int port, boolean initiate, byte[] dataReceived, int size);
 
   /**
    * Closes the SPI port.

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"
@@ -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<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 +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");
 

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 <utility>
+
+#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 <typename T>
+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 <typename T>
+void ToRGB(T val) {
+  std::swap(val[1], val[2]);  // swap G and R
+}
+
+template <typename T>
+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 <typename T>
+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 <typename T>
+void ToBRG(T val) {
+  std::swap(val[0], val[1]);  // swap B and G
+}
+
+template <typename T>
+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 <bool srcAlign, bool dstAlign, ConvertFunc<uint8x16_t*> Convert>
+void ConvertNEON_16(const uint8_t* src, uint8_t* dst) {
+  uint8x16x4_t pixels = Load4<srcAlign>(src);
+  Convert(pixels.val);
+  Store4<dstAlign>(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 <bool srcAlign, bool dstAlign, ConvertFunc<uint8x8_t*> Convert>
+void ConvertNEON_8(const uint8_t* src, uint8_t* dst) {
+  uint8x8x4_t pixels = LoadHalf4<srcAlign>(src);
+  Convert(pixels.val);
+  Store4<dstAlign>(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 <HAL_AddressableLEDColorOrder order, bool srcAlign, bool dstAlign>
+void Convert16Pixels(const uint8_t* src, uint8_t* dst) {
+  switch (order) {
+    case HAL_ALED_RGB:
+      ConvertNEON_16<srcAlign, dstAlign, ToRGB>(src, dst);
+      break;
+    case HAL_ALED_RBG:
+      ConvertNEON_16<srcAlign, dstAlign, ToRBG>(src, dst);
+      break;
+    case HAL_ALED_BGR:
+      ConvertNEON_16<srcAlign, dstAlign, ToBGR>(src, dst);
+      break;
+    case HAL_ALED_BRG:
+      ConvertNEON_16<srcAlign, dstAlign, ToBRG>(src, dst);
+      break;
+    case HAL_ALED_GBR:
+      ConvertNEON_16<srcAlign, dstAlign, ToGBR>(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 <HAL_AddressableLEDColorOrder order, bool srcAlign, bool dstAlign>
+void Convert8Pixels(const uint8_t* src, uint8_t* dst) {
+  switch (order) {
+    case HAL_ALED_RGB:
+      ConvertNEON_8<srcAlign, dstAlign, ToRGB>(src, dst);
+      break;
+    case HAL_ALED_RBG:
+      ConvertNEON_8<srcAlign, dstAlign, ToRBG>(src, dst);
+      break;
+    case HAL_ALED_BGR:
+      ConvertNEON_8<srcAlign, dstAlign, ToBGR>(src, dst);
+      break;
+    case HAL_ALED_BRG:
+      ConvertNEON_8<srcAlign, dstAlign, ToBRG>(src, dst);
+      break;
+    case HAL_ALED_GBR:
+      ConvertNEON_8<srcAlign, dstAlign, ToGBR>(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 <HAL_AddressableLEDColorOrder order, bool srcAlign, bool dstAlign>
+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<order, srcAlign, dstAlign>(src + i, dst + i);
+    }
+    if (aligned < size) {
+      Convert16Pixels<order, false, false>(
+          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<order, srcAlign, dstAlign>(src, dst);
+    if (len > 8) {
+      size_t recopyOffset = (len * 4) - (HA * 4);
+      Convert8Pixels<order, false, false>(
+          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 <HAL_AddressableLEDColorOrder order>
+void ConvertPixels(const uint8_t* src, uint8_t* dst, size_t pixelCount) {
+  if (Aligned(src) && Aligned(dst)) {
+    ConvertPixels<order, true, true>(src, dst, pixelCount);
+  } else if (Aligned(src)) {
+    ConvertPixels<order, true, false>(src, dst, pixelCount);
+  } else if (Aligned(dst)) {
+    ConvertPixels<order, false, true>(src, dst, pixelCount);
+  } else {
+    ConvertPixels<order, false, false>(src, dst, pixelCount);
+  }
+}
+}  // namespace hal::detail

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 <arm_neon.h>
+
+#include <cstddef>
+#include <cstring>
+
+// 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<void*>(reinterpret_cast<size_t>(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 <bool align>
+SIMD_INLINE uint8x8x4_t LoadHalf4(const uint8_t* p);
+
+template <>
+SIMD_INLINE uint8x8x4_t LoadHalf4<false>(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<true>(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<uint8_t*>(__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 <bool align>
+SIMD_INLINE uint8x16x4_t Load4(const uint8_t* p);
+
+template <>
+SIMD_INLINE uint8x16x4_t Load4<false>(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<true>(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<uint8_t*>(__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 <bool align>
+SIMD_INLINE void Store4(uint8_t* p, uint8x16x4_t a);
+
+template <>
+SIMD_INLINE void Store4<false>(uint8_t* p, uint8x16x4_t a) {
+  vst4q_u8(p, a);
+}
+
+template <>
+SIMD_INLINE void Store4<true>(uint8_t* p, uint8x16x4_t a) {
+#if defined(__GNUC__)
+  uint8_t* _p = static_cast<uint8_t*>(__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 <bool align>
+SIMD_INLINE void Store4(uint8_t* p, uint8x8x4_t a);
+
+template <>
+SIMD_INLINE void Store4<false>(uint8_t* p, uint8x8x4_t a) {
+  vst4_u8(p, a);
+}
+
+template <>
+SIMD_INLINE void Store4<true>(uint8_t* p, uint8x8x4_t a) {
+#if defined(__GNUC__)
+  uint8_t* _p = static_cast<uint8_t*>(__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

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<HAL_AddressableLEDHandle>(handle),
+      static_cast<HAL_AddressableLEDColorOrder>(colorOrder), &status);
+  CheckStatus(env, status);
+}
+
 /*
  * Class:     edu_wpi_first_hal_AddressableLEDJNI
  * Method:    setLength

hal/src/main/native/cpp/jni/SPIJNI.cpp

@@ -55,12 +55,12 @@ Java_edu_wpi_first_hal_SPIJNI_spiInitialize
 /*
  * Class:     edu_wpi_first_hal_SPIJNI
  * Method:    spiTransaction
- * Signature: (ILjava/lang/Object;Ljava/lang/Object;B)I
+ * Signature: (ILjava/lang/Object;Ljava/lang/Object;I)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SPIJNI_spiTransaction
   (JNIEnv* env, jclass, jint port, jobject dataToSend, jobject dataReceived,
-   jbyte size)
+   jint size)
 {
   uint8_t* dataToSendPtr = nullptr;
   if (dataToSend != nullptr) {
@@ -77,12 +77,12 @@ Java_edu_wpi_first_hal_SPIJNI_spiTransaction
 /*
  * Class:     edu_wpi_first_hal_SPIJNI
  * Method:    spiTransactionB
- * Signature: (I[B[BB)I
+ * Signature: (I[B[BI)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SPIJNI_spiTransactionB
   (JNIEnv* env, jclass, jint port, jbyteArray dataToSend,
-   jbyteArray dataReceived, jbyte size)
+   jbyteArray dataReceived, jint size)
 {
   if (size < 0) {
     ThrowIllegalArgumentException(env, "SPIJNI.spiTransactionB() size < 0");
@@ -104,11 +104,11 @@ Java_edu_wpi_first_hal_SPIJNI_spiTransactionB
 /*
  * Class:     edu_wpi_first_hal_SPIJNI
  * Method:    spiWrite
- * Signature: (ILjava/lang/Object;B)I
+ * Signature: (ILjava/lang/Object;I)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SPIJNI_spiWrite
-  (JNIEnv* env, jclass, jint port, jobject dataToSend, jbyte size)
+  (JNIEnv* env, jclass, jint port, jobject dataToSend, jint size)
 {
   uint8_t* dataToSendPtr = nullptr;
   if (dataToSend != nullptr) {
@@ -123,11 +123,11 @@ Java_edu_wpi_first_hal_SPIJNI_spiWrite
 /*
  * Class:     edu_wpi_first_hal_SPIJNI
  * Method:    spiWriteB
- * Signature: (I[BB)I
+ * Signature: (I[BI)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SPIJNI_spiWriteB
-  (JNIEnv* env, jclass, jint port, jbyteArray dataToSend, jbyte size)
+  (JNIEnv* env, jclass, jint port, jbyteArray dataToSend, jint size)
 {
   jint retVal = HAL_WriteSPI(static_cast<HAL_SPIPort>(port),
                              reinterpret_cast<const uint8_t*>(
@@ -139,12 +139,12 @@ Java_edu_wpi_first_hal_SPIJNI_spiWriteB
 /*
  * Class:     edu_wpi_first_hal_SPIJNI
  * Method:    spiRead
- * Signature: (IZLjava/lang/Object;B)I
+ * Signature: (IZLjava/lang/Object;I)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SPIJNI_spiRead
   (JNIEnv* env, jclass, jint port, jboolean initiate, jobject dataReceived,
-   jbyte size)
+   jint size)
 {
   if (size < 0) {
     ThrowIllegalArgumentException(env, "SPIJNI.spiRead() size < 0");
@@ -169,12 +169,12 @@ Java_edu_wpi_first_hal_SPIJNI_spiRead
 /*
  * Class:     edu_wpi_first_hal_SPIJNI
  * Method:    spiReadB
- * Signature: (IZ[BB)I
+ * Signature: (IZ[BI)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SPIJNI_spiReadB
   (JNIEnv* env, jclass, jint port, jboolean initiate, jbyteArray dataReceived,
-   jbyte size)
+   jint size)
 {
   if (size < 0) {
     ThrowIllegalArgumentException(env, "SPIJNI.spiReadB() size < 0");

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.
  *
@@ -77,8 +88,8 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
 /**
  * Sets the bit timing.
  *
- * <p>By default, the driver is set up to drive WS2812Bs, so nothing needs to
- * be set for those.
+ * <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing
+ * needs to be set for those.
  *
  * @param[in] handle the Addressable LED handle
  * @param[in] highTime0NanoSeconds high time for 0 bit (default 400ns)
@@ -98,7 +109,7 @@ void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
  * Sets the sync time.
  *
  * <p>The sync time is the time to hold output so LEDs enable. Default set for
- * WS2812B.
+ * WS2812B and WS2815.
  *
  * @param[in] handle the Addressable LED handle
  * @param[in] syncTimeMicroSeconds the sync time (default 280us)

hal/src/main/native/include/hal/AddressableLEDTypes.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <hal/Types.h>
 #include <stdint.h>
 
 /** 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<int32_t>(order);
+}
+#endif

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) {

shared/bazel/compiler_flags/systemcore_flags.rc

@@ -0,0 +1,10 @@
+
+build:systemcore --config=base_linux
+
+build:systemcore --platforms=@rules_bzlmodrio_toolchains//platforms/systemcore
+build:systemcore --build_tag_filters=-no-bookworm
+build:systemcore --features=compiler_param_file
+build:systemcore --platform_suffix=systemcore
+build:systemcore --incompatible_enable_cc_toolchain_resolution
+
+build:systemcore --cxxopt=-Wno-error=deprecated-declarations

sysid/src/main/native/cpp/App.cpp

@@ -106,8 +106,10 @@ void Application(std::string_view saveDir) {
   auto analyzer = std::make_unique<sysid::Analyzer>(storage, gLogger);
 
   logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); });
-  dataSelector->testdata = [_analyzer = analyzer.get()](auto data) {
+  dataSelector->testdata = [_analyzer = analyzer.get(),
+                            ds = dataSelector.get()](auto data) {
     _analyzer->m_data = data;
+    _analyzer->SetMissingTests(ds->m_missingTests);
     _analyzer->AnalyzeData();
   };
 

sysid/src/main/native/cpp/view/Analyzer.cpp

@@ -10,6 +10,7 @@
 #include <numbers>
 #include <string>
 #include <thread>
+#include <vector>
 
 #include <fmt/format.h>
 #include <glass/Context.h>
@@ -251,6 +252,13 @@ void Analyzer::Display() {
       }
       break;
     }
+    case AnalyzerState::kMissingTestsError: {
+      CreateErrorPopup(m_errorPopup, m_exception);
+      if (!m_errorPopup) {
+        m_state = AnalyzerState::kWaitingForData;
+      }
+      break;
+    }
     case AnalyzerState::kGeneralDataError:
     case AnalyzerState::kTestDurationError:
     case AnalyzerState::kVelocityThresholdError: {
@@ -269,6 +277,9 @@ void Analyzer::Display() {
 void Analyzer::PrepareData() {
   WPI_INFO(m_logger, "{}", "Preparing data");
   try {
+    if (m_missingTests.size() > 0) {
+      throw sysid::MissingTestsError{m_missingTests};
+    }
     m_manager->PrepareData();
     UpdateFeedforwardGains();
     UpdateFeedbackGains();
@@ -281,6 +292,9 @@ void Analyzer::PrepareData() {
   } catch (const sysid::NoDynamicDataError& e) {
     m_state = AnalyzerState::kTestDurationError;
     HandleError(e.what());
+  } catch (const sysid::MissingTestsError& e) {
+    m_state = AnalyzerState::kMissingTestsError;
+    HandleError(e.what());
   } catch (const AnalysisManager::FileReadingError& e) {
     m_state = AnalyzerState::kFileError;
     HandleError(e.what());
@@ -324,6 +338,10 @@ void Analyzer::HandleError(std::string_view msg) {
   PrepareRawGraphs();
 }
 
+void Analyzer::SetMissingTests(const std::vector<std::string>& missingTests) {
+  m_missingTests = missingTests;
+}
+
 void Analyzer::DisplayGraphs() {
   ImGui::SetNextWindowPos(ImVec2{kDiagnosticPlotWindowPos},
                           ImGuiCond_FirstUseEver);

sysid/src/main/native/cpp/view/DataSelector.cpp

@@ -6,6 +6,7 @@
 
 #include <algorithm>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
@@ -111,6 +112,7 @@ void DataSelector::Display() {
           continue;
         }
         WPI_INFO(m_logger, "Loaded test state {}", it2->first);
+        m_executedTests.insert(it2->first);
         ++it2;
       }
       if (it->second.empty()) {
@@ -132,6 +134,15 @@ void DataSelector::Display() {
     return;
   }
 
+  if (m_executedTests.size() < 4 && !m_testCountValidated) {
+    for (auto test : kValidTests) {
+      if (!m_executedTests.contains(test)) {
+        m_missingTests.push_back(test);
+        m_testCountValidated = true;
+      }
+    }
+  }
+
 #if 0
   // Test filtering
   if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) {

sysid/src/main/native/include/sysid/analysis/FilteringUtils.h

@@ -4,15 +4,16 @@
 
 #pragma once
 
-#include <cmath>
 #include <exception>
 #include <functional>
 #include <string>
 #include <string_view>
 #include <tuple>
+#include <utility>
 #include <vector>
 
 #include <fmt/format.h>
+#include <fmt/ranges.h>
 #include <frc/filter/LinearFilter.h>
 #include <units/time.h>
 #include <wpi/StringMap.h>
@@ -68,6 +69,25 @@ class NoQuasistaticDataError : public std::exception {
   }
 };
 
+/**
+ * Exception for not all tests being present.
+ */
+class MissingTestsError : public std::exception {
+ public:
+  explicit MissingTestsError(std::vector<std::string> MissingTests)
+      : missingTests(std::move(MissingTests)) {
+    errorString = fmt::format(
+        "The following tests were not detected: {}. Make sure to perform all "
+        "four tests as described in the SysId documentation.",
+        fmt::join(missingTests, ", "));
+  }
+  const char* what() const noexcept override { return errorString.c_str(); }
+
+ private:
+  std::vector<std::string> missingTests;
+  std::string errorString;
+};
+
 /**
  * Exception for Dynamic Data being completely removed.
  */

sysid/src/main/native/include/sysid/view/Analyzer.h

@@ -8,6 +8,7 @@
 #include <string>
 #include <string_view>
 #include <thread>
+#include <vector>
 
 #include <glass/View.h>
 #include <implot.h>
@@ -46,6 +47,7 @@ class Analyzer : public glass::View {
     kVelocityThresholdError,
     kTestDurationError,
     kGeneralDataError,
+    kMissingTestsError,
     kFileError
   };
   /**
@@ -91,6 +93,11 @@ class Analyzer : public glass::View {
    */
   void AnalyzeData();
 
+  /**
+   * Used by DataSelector to import any missing tests.
+   */
+  void SetMissingTests(const std::vector<std::string>& missingTests);
+
  private:
   /**
    * Kills the data preparation thread
@@ -199,6 +206,7 @@ class Analyzer : public glass::View {
 
   // Stores the exception message.
   std::string m_exception;
+  std::vector<std::string> m_missingTests;
 
   bool m_calcDefaults = false;
 

sysid/src/main/native/include/sysid/view/DataSelector.h

@@ -7,6 +7,7 @@
 #include <functional>
 #include <future>
 #include <map>
+#include <set>
 #include <string>
 #include <utility>
 #include <vector>
@@ -55,6 +56,7 @@ class DataSelector : public glass::View {
    * Called when new test data is loaded.
    */
   std::function<void(TestData)> testdata;
+  std::vector<std::string> m_missingTests;
 
  private:
   wpi::Logger& m_logger;
@@ -74,6 +76,11 @@ class DataSelector : public glass::View {
   int m_selectedAnalysis = 0;
   std::future<TestData> m_testdataFuture;
   std::vector<std::string> m_testdataStats;
+  std::set<std::string> kValidTests = {"quasistatic-forward",
+                                       "quasistatic-reverse", "dynamic-forward",
+                                       "dynamic-reverse"};
+  std::set<std::string> m_executedTests;
+  bool m_testCountValidated = false;
 
   static Tests LoadTests(const glass::DataLogReaderEntry& testStateEntry);
   TestData BuildTestData();

upstream_utils/eigen_patches/0001-Disable-warnings.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 18 May 2022 09:14:24 -0700
-Subject: [PATCH 1/2] Disable warnings
+Subject: [PATCH 1/3] Disable warnings
 
 ---
  Eigen/src/Core/util/DisableStupidWarnings.h | 6 ++++++

upstream_utils/eigen_patches/0002-Intellisense-fix.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Fri, 20 Jan 2023 23:41:56 -0800
-Subject: [PATCH 2/2] Intellisense fix
+Subject: [PATCH 2/3] Intellisense fix
 
 ---
  Eigen/src/Core/util/ConfigureVectorization.h | 7 +++++++

upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch

@@ -0,0 +1,305 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sun, 12 Jan 2025 21:04:07 -0800
+Subject: [PATCH 3/3] Make assignment constexpr
+
+---
+ Eigen/src/Core/AssignEvaluator.h             | 165 +++++++++++--------
+ Eigen/src/Core/EigenBase.h                   |   2 +-
+ Eigen/src/Core/functors/AssignmentFunctors.h |   2 +-
+ 3 files changed, 102 insertions(+), 67 deletions(-)
+
+diff --git a/Eigen/src/Core/AssignEvaluator.h b/Eigen/src/Core/AssignEvaluator.h
+index f7f0b238b8ca70bbc9100262479cc1dbebab9979..9c2436afa7fe98692a036e6ef255ed104a5bf388 100644
+--- a/Eigen/src/Core/AssignEvaluator.h
++++ b/Eigen/src/Core/AssignEvaluator.h
+@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling {
+     DstAlignment = Kernel::AssignmentTraits::DstAlignment
+   };
+ 
+-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
++  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
+     kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
+     enum { NextIndex = Index + unpacket_traits<PacketType>::size };
+     copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
+@@ -431,17 +431,25 @@ struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling> {
+ template <typename Kernel>
+ struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling> {
+   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
+-    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+-    typedef typename Kernel::PacketType PacketType;
+-
+-    enum {
+-      size = DstXprType::SizeAtCompileTime,
+-      packetSize = unpacket_traits<PacketType>::size,
+-      alignedSize = (int(size) / packetSize) * packetSize
+-    };
+-
+-    copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
+-    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
++    if (internal::is_constant_evaluated()) {
++      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
++        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
++          kernel.assignCoeffByOuterInner(outer, inner);
++        }
++      }
++    } else {
++      typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
++      typedef typename Kernel::PacketType PacketType;
++
++      enum {
++        size = DstXprType::SizeAtCompileTime,
++        packetSize = unpacket_traits<PacketType>::size,
++        alignedSize = (int(size) / packetSize) * packetSize
++      };
++
++      copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
++      copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
++    }
+   }
+ };
+ 
+@@ -465,9 +473,17 @@ struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling> {
+ 
+ template <typename Kernel>
+ struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling> {
+-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
+-    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+-    copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
++  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
++    if (internal::is_constant_evaluated()) {
++      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
++        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
++          kernel.assignCoeffByOuterInner(outer, inner);
++        }
++      }
++    } else {
++      typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
++      copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
++    }
+   }
+ };
+ 
+@@ -498,8 +514,16 @@ struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling> {
+ template <typename Kernel>
+ struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
+   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
+-    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+-    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
++    if (internal::is_constant_evaluated()) {
++      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
++        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
++          kernel.assignCoeffByOuterInner(outer, inner);
++        }
++      }
++    } else {
++      typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
++      copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
++    }
+   }
+ };
+ 
+@@ -510,41 +534,49 @@ struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
+ template <typename Kernel>
+ struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling> {
+   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
+-    typedef typename Kernel::Scalar Scalar;
+-    typedef typename Kernel::PacketType PacketType;
+-    enum {
+-      packetSize = unpacket_traits<PacketType>::size,
+-      requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
+-      alignable =
+-          packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
+-      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
+-      dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
+-    };
+-    const Scalar* dst_ptr = kernel.dstDataPtr();
+-    if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
+-      // the pointer is not aligned-on scalar, so alignment is not possible
+-      return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
+-    }
+-    const Index packetAlignedMask = packetSize - 1;
+-    const Index innerSize = kernel.innerSize();
+-    const Index outerSize = kernel.outerSize();
+-    const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
+-    Index alignedStart =
+-        ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
+-
+-    for (Index outer = 0; outer < outerSize; ++outer) {
+-      const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
+-      // do the non-vectorizable part of the assignment
+-      for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
+-
+-      // do the vectorizable part of the assignment
+-      for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
+-        kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
+-
+-      // do the non-vectorizable part of the assignment
+-      for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
+-
+-      alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
++    if (internal::is_constant_evaluated()) {
++      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
++        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
++          kernel.assignCoeffByOuterInner(outer, inner);
++        }
++      }
++    } else {
++      typedef typename Kernel::Scalar Scalar;
++      typedef typename Kernel::PacketType PacketType;
++      enum {
++        packetSize = unpacket_traits<PacketType>::size,
++        requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
++        alignable =
++            packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
++        dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
++        dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
++      };
++      const Scalar* dst_ptr = kernel.dstDataPtr();
++      if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
++        // the pointer is not aligned-on scalar, so alignment is not possible
++        return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
++      }
++      const Index packetAlignedMask = packetSize - 1;
++      const Index innerSize = kernel.innerSize();
++      const Index outerSize = kernel.outerSize();
++      const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
++      Index alignedStart =
++          ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
++
++      for (Index outer = 0; outer < outerSize; ++outer) {
++        const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
++        // do the non-vectorizable part of the assignment
++        for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
++
++        // do the vectorizable part of the assignment
++        for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
++          kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
++
++        // do the non-vectorizable part of the assignment
++        for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
++
++        alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
++      }
+     }
+   }
+ };
+@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel {
+   typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
+   typedef typename AssignmentTraits::PacketType PacketType;
+ 
+-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst,
+-                                                                        const SrcEvaluatorType& src,
+-                                                                        const Functor& func, DstXprType& dstExpr)
++  EIGEN_DEVICE_FUNC
++  EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
++                                                                const Functor& func, DstXprType& dstExpr)
+       : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) {
+ #ifdef EIGEN_DEBUG_ASSIGN
+     AssignmentTraits::debug();
+@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel {
+   EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
+ 
+   /// Assign src(row,col) to dst(row,col) through the assignment functor.
+-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) {
++  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) {
+     m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col));
+   }
+ 
+@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel {
+   }
+ 
+   /// \sa assignCoeff(Index,Index)
+-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) {
++  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) {
+     Index row = rowIndexByOuterInner(outer, inner);
+     Index col = colIndexByOuterInner(outer, inner);
+     assignCoeff(row, col);
+@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel {
+     assignPacket<StoreMode, LoadMode, Packet>(row, col);
+   }
+ 
+-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) {
++  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) {
+     typedef typename DstEvaluatorType::ExpressionTraits Traits;
+     return int(Traits::RowsAtCompileTime) == 1          ? 0
+            : int(Traits::ColsAtCompileTime) == 1        ? inner
+@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel {
+                                                         : inner;
+   }
+ 
+-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) {
++  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) {
+     typedef typename DstEvaluatorType::ExpressionTraits Traits;
+     return int(Traits::ColsAtCompileTime) == 1          ? 0
+            : int(Traits::RowsAtCompileTime) == 1        ? inner
+@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co
+ }
+ 
+ template <typename DstXprType, typename SrcXprType, typename T1, typename T2>
+-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
+-                                                             const internal::assign_op<T1, T2>& /*func*/) {
++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
++                                                                       const internal::assign_op<T1, T2>& /*func*/) {
+   Index dstRows = src.rows();
+   Index dstCols = src.cols();
+   if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols);
+@@ -790,7 +822,7 @@ struct Assignment;
+ // not has to bother about these annoying details.
+ 
+ template <typename Dst, typename Src>
+-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) {
++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) {
+   call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
+ }
+ template <typename Dst, typename Src>
+@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(
+ }
+ 
+ template <typename Dst, typename Src, typename Func>
+-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(
++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(
+     Dst& dst, const Src& src, const Func& func, std::enable_if_t<!evaluator_assume_aliasing<Src>::value, void*> = 0) {
+   call_assignment_no_alias(dst, src, func);
+ }
+@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src);
+ // both partial specialization+SFINAE without ambiguous specialization
+ template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+ struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak> {
+-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
++  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src,
++                                                                  const Functor& func) {
+ #ifndef EIGEN_NO_DEBUG
+-    internal::check_for_aliasing(dst, src);
++    if (!internal::is_constant_evaluated()) {
++      internal::check_for_aliasing(dst, src);
++    }
+ #endif
+ 
+     call_dense_assignment_loop(dst, src, func);
+diff --git a/Eigen/src/Core/EigenBase.h b/Eigen/src/Core/EigenBase.h
+index 6d167006a094181fa3693b19f6b9daeb6f2afb79..894bfc13b15eb994abd90f100da15de5bd8b22b7 100644
+--- a/Eigen/src/Core/EigenBase.h
++++ b/Eigen/src/Core/EigenBase.h
+@@ -50,7 +50,7 @@ struct EigenBase {
+   /** \returns a const reference to the derived object */
+   EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast<const Derived*>(this); }
+ 
+-  EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const {
++  EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const {
+     return *static_cast<Derived*>(const_cast<EigenBase*>(this));
+   }
+   EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast<const Derived*>(this); }
+diff --git a/Eigen/src/Core/functors/AssignmentFunctors.h b/Eigen/src/Core/functors/AssignmentFunctors.h
+index 09d1da8ca2bcb41384520f46e2b793ba8b28a798..3687bb20db4dfe1a2f6cf1342b4fcbd8f91f1f68 100644
+--- a/Eigen/src/Core/functors/AssignmentFunctors.h
++++ b/Eigen/src/Core/functors/AssignmentFunctors.h
+@@ -23,7 +23,7 @@ namespace internal {
+  */
+ template <typename DstScalar, typename SrcScalar>
+ struct assign_op {
+-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
++  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
+ 
+   template <int Alignment, typename Packet>
+   EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {

upstream_utils/stack_walker.py

@@ -1,65 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import shutil
-import subprocess
-
-from upstream_utils import Lib
-
-
-def crlf_to_lf():
-    for root, _, files in os.walk("."):
-        if ".git" in root:
-            continue
-
-        for fname in files:
-            filename = os.path.join(root, fname)
-            print(f"Converting CRLF -> LF for {filename}")
-            with open(filename, "rb") as f:
-                content = f.read()
-                content = content.replace(b"\r\n", b"\n")
-
-                with open(filename, "wb") as f:
-                    f.write(content)
-
-    subprocess.check_call(["git", "add", "-A"])
-    subprocess.check_call(["git", "commit", "-m", "Fix line endings"])
-
-
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
-
-    shutil.copy(
-        os.path.join("Main", "StackWalker", "StackWalker.h"),
-        os.path.join(wpiutil, "src/main/native/windows/StackWalker.h"),
-    )
-
-    shutil.copy(
-        os.path.join("Main", "StackWalker", "StackWalker.cpp"),
-        os.path.join(wpiutil, "src/main/native/windows/StackWalker.cpp"),
-    )
-
-
-def main():
-    name = "stack_walker"
-    url = "https://github.com/JochenKalmbach/StackWalker"
-    tag = "5b0df7a4db8896f6b6dc45d36e383c52577e3c6b"
-
-    patch_options = {
-        "ignore_whitespace": True,
-    }
-
-    stack_walker = Lib(
-        name,
-        url,
-        tag,
-        copy_upstream_src,
-        patch_options,
-        pre_patch_hook=crlf_to_lf,
-        pre_patch_commits=1,
-    )
-    stack_walker.main()
-
-
-if __name__ == "__main__":
-    main()

upstream_utils/stack_walker_patches/0001-Add-advapi-pragma.patch

@@ -1,21 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Thad House <thadhouse1@gmail.com>
-Date: Sat, 22 Jul 2023 13:03:13 -0700
-Subject: [PATCH] Add advapi pragma
-
----
- Main/StackWalker/StackWalker.cpp | 1 +
- 1 file changed, 1 insertion(+)
-
-diff --git a/Main/StackWalker/StackWalker.cpp b/Main/StackWalker/StackWalker.cpp
-index 89545f8612d0d099d48fcf4184a2f2a30cf8577f..b2bcbaa447c5db1a3bcc155fb317ebc8a8050e79 100644
---- a/Main/StackWalker/StackWalker.cpp
-+++ b/Main/StackWalker/StackWalker.cpp
-@@ -91,6 +91,7 @@
- #include <new>
- 
- #pragma comment(lib, "version.lib") // for "VerQueryValue"
-+#pragma comment(lib, "Advapi32.lib") // for "GetUserName"
- 
- #pragma warning(disable : 4826)
- #if _MSC_VER >= 1900

wpical/src/main/native/cpp/WPIcal.cpp

@@ -9,12 +9,14 @@
 
 #include <fstream>
 #include <iostream>
+#include <map>
 #include <memory>
 #include <numbers>
 #include <string>
 #include <vector>
 
 #include <GLFW/glfw3.h>
+#include <fmt/format.h>
 #include <imgui.h>
 #include <portable-file-dialogs.h>
 #include <tagpose.h>
@@ -58,6 +60,112 @@ void drawCheck() {
   ImGui::NewLine();
 }
 
+void processFileSelector(std::unique_ptr<pfd::open_file>& selector,
+                         std::string& selected_file) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_file = selectedFiles[0];
+    }
+    selector.reset();
+  }
+}
+
+void processFilesSelector(std::unique_ptr<pfd::open_file>& selector,
+                          std::vector<std::string>& selected_files) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_files = selectedFiles;
+    }
+    selector.reset();
+  }
+}
+
+void processDirectorySelector(std::unique_ptr<pfd::select_folder>& selector,
+                              std::string& selected_directory) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_directory = selectedFiles;
+    }
+    selector.reset();
+  }
+}
+
+void openFileButton(const char* text, std::string& selected_file,
+                    std::unique_ptr<pfd::open_file>& selector,
+                    const std::string& file_type,
+                    const std::string& file_extensions) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::open_file>(
+        "Select File", "", std::vector<std::string>{file_type, file_extensions},
+        pfd::opt::none);
+  }
+}
+
+void openFilesButton(const char* text, std::vector<std::string>& selected_files,
+                     std::unique_ptr<pfd::open_file>& selector,
+                     const std::string& file_type,
+                     const std::string& file_extensions) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::open_file>(
+        "Select File", "", std::vector<std::string>{file_type, file_extensions},
+        pfd::opt::multiselect);
+  }
+}
+
+void openDirectoryButton(const char* text,
+                         std::unique_ptr<pfd::select_folder>& selector,
+                         std::string& selected_directory) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::select_folder>("Select Directory", "");
+  }
+}
+
+std::string getFileName(std::string path) {
+  size_t lastSlash = path.find_last_of("/\\");
+  size_t lastDot = path.find_last_of(".");
+  return path.substr(lastSlash + 1, lastDot - lastSlash - 1);
+}
+
+static bool EmitEntryTarget(int tag_id, std::string& file) {
+  if (!file.empty()) {
+    auto text = fmt::format("{}: {}", tag_id, file);
+    ImGui::TextUnformatted(text.c_str());
+  } else {
+    ImGui::Text("Tag ID %i: <none (DROP HERE)>", tag_id);
+  }
+  bool rv = false;
+  if (ImGui::BeginDragDropTarget()) {
+    if (const ImGuiPayload* payload =
+            ImGui::AcceptDragDropPayload("FieldCalibration")) {
+      file = *(std::string*)payload->Data;
+      rv = true;
+    }
+    ImGui::EndDragDropTarget();
+  }
+  return rv;
+}
+
+void saveCalibration(wpi::json& field, std::string& output_directory,
+                     std::string output_name, bool& isCalibrating) {
+  if (!field.empty() && !output_directory.empty()) {
+    std::cout << "Saving calibration to " << output_directory << std::endl;
+    std::ofstream out(output_directory + "/" + output_name + ".json");
+    out << field.dump(4);
+    out.close();
+
+    std::ofstream fmap(output_directory + "/" + output_name + ".fmap");
+    fmap << fmap::convertfmap(field).dump(4);
+    fmap.close();
+
+    field.clear();
+    output_directory.clear();
+    isCalibrating = false;
+  }
+}
+
 static void DisplayGui() {
   ImGui::GetStyle().WindowRounding = 0;
 
@@ -82,19 +190,28 @@ static void DisplayGui() {
   ImGui::EndMenuBar();
 
   static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
-  static std::string selected_camera_intrinsics;
-
   static std::unique_ptr<pfd::open_file> field_map_selector;
-  static std::string selected_field_map;
+  static std::unique_ptr<pfd::open_file> output_calibration_json_selector;
+  static std::unique_ptr<pfd::open_file> combination_calibrations_selector;
 
   static std::unique_ptr<pfd::select_folder>
       field_calibration_directory_selector;
-  static std::string selected_field_calibration_directory;
-
   static std::unique_ptr<pfd::select_folder> download_directory_selector;
-  static std::string selected_download_directory;
 
-  static std::string calibration_json_path;
+  static wpi::json field_calibration_json;
+  static wpi::json field_combination_json;
+
+  static std::string selected_camera_intrinsics;
+  static std::string selected_field_map;
+  static std::string selected_field_calibration_directory;
+  static std::string selected_download_directory;
+  static std::string output_calibration_json_path;
+  static std::vector<std::string> selected_combination_calibrations;
+
+  static std::map<int, std::string> combiner_map;
+  static int current_combiner_tag_id = 0;
+
+  static bool isCalibrating = false;
 
   cameracalibration::CameraModel cameraModel = {
       .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
@@ -118,13 +235,12 @@ static void DisplayGui() {
 
   static Fieldmap currentCalibrationMap;
   static Fieldmap currentReferenceMap;
+  static Fieldmap currentCombinerMap;
 
   // camera matrix selector button
-  if (ImGui::Button("Upload Camera Intrinsics")) {
-    camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-        "Select Camera Intrinsics JSON", "",
-        std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
-  }
+  openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics,
+                 camera_intrinsics_selector, "JSON Files", "*.json");
+  processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics);
 
   ImGui::SameLine();
   ImGui::Text("Or");
@@ -136,50 +252,25 @@ static void DisplayGui() {
     ImGui::OpenPopup("Camera Calibration");
   }
 
-  if (camera_intrinsics_selector) {
-    auto selectedFiles = camera_intrinsics_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_camera_intrinsics = selectedFiles[0];
-    }
-    camera_intrinsics_selector.reset();
-  }
-
   if (!selected_camera_intrinsics.empty()) {
     drawCheck();
   }
 
   // field json selector button
-  if (ImGui::Button("Select Field Map JSON")) {
-    field_map_selector = std::make_unique<pfd::open_file>(
-        "Select Json File", "",
-        std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
-  }
-
-  if (field_map_selector) {
-    auto selectedFiles = field_map_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_field_map = selectedFiles[0];
-    }
-    field_map_selector.reset();
-  }
+  openFileButton("Select Field Map JSON", selected_field_map,
+                 field_map_selector, "JSON Files", "*.json");
+  processFileSelector(field_map_selector, selected_field_map);
 
   if (!selected_field_map.empty()) {
     drawCheck();
   }
 
   // field calibration directory selector button
-  if (ImGui::Button("Select Field Calibration Folder")) {
-    field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
-        "Select Field Calibration Folder", "");
-  }
-
-  if (field_calibration_directory_selector) {
-    auto selectedFiles = field_calibration_directory_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_field_calibration_directory = selectedFiles;
-    }
-    field_calibration_directory_selector.reset();
-  }
+  openDirectoryButton("Select Field Calibration Directory",
+                      field_calibration_directory_selector,
+                      selected_field_calibration_directory);
+  processDirectorySelector(field_calibration_directory_selector,
+                           selected_field_calibration_directory);
 
   if (!selected_field_calibration_directory.empty()) {
     drawCheck();
@@ -191,46 +282,35 @@ static void DisplayGui() {
 
   // calibrate button
   if (ImGui::Button("Calibrate!!!")) {
-    if (!selected_field_calibration_directory.empty() &&
-        !selected_camera_intrinsics.empty() && !selected_field_map.empty()) {
+    int calibrationOutput = fieldcalibration::calibrate(
+        selected_field_calibration_directory.c_str(), field_calibration_json,
+        selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
+        showDebug);
+
+    if (calibrationOutput == 1) {
+      ImGui::OpenPopup("Field Calibration Error");
+    }
+
+    if (selected_download_directory.empty() &&
+        !field_calibration_json.empty() && !download_directory_selector) {
       download_directory_selector =
           std::make_unique<pfd::select_folder>("Select Download Folder", "");
-      if (download_directory_selector) {
-        auto selectedFiles = download_directory_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_download_directory = selectedFiles;
-        }
-        download_directory_selector.reset();
-      }
-
-      calibration_json_path = selected_download_directory + "/output.json";
-
-      int calibrationOutput = fieldcalibration::calibrate(
-          selected_field_calibration_directory.c_str(), calibration_json_path,
-          selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
-          showDebug);
-
-      if (calibrationOutput == 1) {
-        ImGui::OpenPopup("Field Calibration Error");
-      } else if (calibrationOutput == 0) {
-        std::ifstream caljsonpath(calibration_json_path);
-        try {
-          wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
-          std::ofstream out(selected_download_directory + "/output.fmap");
-          out << fmap.dump(4);
-          out.close();
-          ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
-          ImGui::OpenPopup("Visualize Calibration");
-        } catch (...) {
-          ImGui::OpenPopup("Fmap Conversion Error");
-        }
-      }
     }
   }
+
+  processDirectorySelector(download_directory_selector,
+                           selected_download_directory);
+  saveCalibration(field_calibration_json, selected_download_directory,
+                  "field_calibration", isCalibrating);
+
   if (ImGui::Button("Visualize")) {
     ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
     ImGui::OpenPopup("Visualize Calibration");
   }
+  if (ImGui::Button("Combine Calibrations")) {
+    ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+    ImGui::OpenPopup("Combine Calibrations");
+  }
   if (selected_field_calibration_directory.empty() ||
       selected_camera_intrinsics.empty() || selected_field_map.empty()) {
     ImGui::TextWrapped(
@@ -320,21 +400,11 @@ static void DisplayGui() {
     }
 
     if (mrcal) {
-      if (ImGui::Button("Select Camera Calibration Video")) {
-        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-            "Select Camera Calibration Video", "",
-            std::vector<std::string>{"Video Files",
-                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
-            pfd::opt::none);
-      }
-
-      if (camera_intrinsics_selector) {
-        auto selectedFiles = camera_intrinsics_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_camera_intrinsics = selectedFiles[0];
-        }
-        camera_intrinsics_selector.reset();
-      }
+      openFileButton("Select Camera Calibration Video",
+                     selected_camera_intrinsics, camera_intrinsics_selector,
+                     "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
+      processFileSelector(camera_intrinsics_selector,
+                          selected_camera_intrinsics);
 
       ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
       ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -379,21 +449,11 @@ static void DisplayGui() {
         }
       }
     } else {
-      if (ImGui::Button("Select Camera Calibration Video")) {
-        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-            "Select Camera Calibration Video", "",
-            std::vector<std::string>{"Video Files",
-                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
-            pfd::opt::none);
-      }
-
-      if (camera_intrinsics_selector) {
-        auto selectedFiles = camera_intrinsics_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_camera_intrinsics = selectedFiles[0];
-        }
-        camera_intrinsics_selector.reset();
-      }
+      openFileButton("Select Camera Calibration Video",
+                     selected_camera_intrinsics, camera_intrinsics_selector,
+                     "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
+      processFileSelector(camera_intrinsics_selector,
+                          selected_camera_intrinsics);
 
       ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
       ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -446,26 +506,19 @@ static void DisplayGui() {
   // visualize calibration popup
   if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
                              ImGuiWindowFlags_AlwaysAutoResize)) {
-    if (ImGui::Button("Load Calibrated Field")) {
-      calibration_json_path =
-          std::make_unique<pfd::open_file>(
-              "Select Json File", "",
-              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
-              ->result()[0];
-    }
+    openFileButton("Select Calibration JSON", output_calibration_json_path,
+                   output_calibration_json_selector, "JSON", "*.json");
+    processFileSelector(output_calibration_json_selector,
+                        output_calibration_json_path);
 
-    if (!calibration_json_path.empty()) {
+    if (!output_calibration_json_path.empty()) {
       ImGui::SameLine();
       drawCheck();
     }
 
-    if (ImGui::Button("Load Reference Field")) {
-      selected_field_map =
-          std::make_unique<pfd::open_file>(
-              "Select Json File", "",
-              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
-              ->result()[0];
-    }
+    openFileButton("Select Ideal Field Map", selected_field_map,
+                   field_map_selector, "JSON", "*.json");
+    processFileSelector(field_map_selector, selected_field_map);
 
     if (!selected_field_map.empty()) {
       ImGui::SameLine();
@@ -477,58 +530,76 @@ static void DisplayGui() {
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
     ImGui::InputInt("Reference Tag", &referenceTag);
 
-    if (!calibration_json_path.empty() && !selected_field_map.empty()) {
-      std::ifstream calJson(calibration_json_path);
+    if (!output_calibration_json_path.empty() && !selected_field_map.empty()) {
+      std::ifstream calJson(output_calibration_json_path);
       std::ifstream refJson(selected_field_map);
 
       currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
       currentReferenceMap = Fieldmap(wpi::json::parse(refJson));
 
-      double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
-                     currentCalibrationMap.getTag(focusedTag).xPos;
-      double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
-                     currentCalibrationMap.getTag(focusedTag).yPos;
-      double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
-                     currentCalibrationMap.getTag(focusedTag).zPos;
-      double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
-                       currentCalibrationMap.getTag(focusedTag).yawRot;
-      double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
-                         currentCalibrationMap.getTag(focusedTag).pitchRot;
-      double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
-                        currentCalibrationMap.getTag(focusedTag).rollRot;
+      if (currentCalibrationMap.getNumTags() !=
+          currentReferenceMap.getNumTags()) {
+        ImGui::TextWrapped(
+            "The number of tags in the calibration output and the ideal field "
+            "map "
+            "do not match. Please ensure that the calibration output and ideal "
+            "field "
+            "map have the same number of tags.");
+      } else if (currentReferenceMap.hasTag(focusedTag) &&
+                 currentReferenceMap.hasTag(referenceTag)) {
+        double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
+                       currentCalibrationMap.getTag(focusedTag).xPos;
+        double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
+                       currentCalibrationMap.getTag(focusedTag).yPos;
+        double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
+                       currentCalibrationMap.getTag(focusedTag).zPos;
+        double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
+                         currentCalibrationMap.getTag(focusedTag).yawRot;
+        double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
+                           currentCalibrationMap.getTag(focusedTag).pitchRot;
+        double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
+                          currentCalibrationMap.getTag(focusedTag).rollRot;
 
-      double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
-                    currentCalibrationMap.getTag(focusedTag).xPos;
-      double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
-                    currentCalibrationMap.getTag(focusedTag).yPos;
-      double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
-                    currentCalibrationMap.getTag(focusedTag).zPos;
+        double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
+                      currentCalibrationMap.getTag(focusedTag).xPos;
+        double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
+                      currentCalibrationMap.getTag(focusedTag).yPos;
+        double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
+                      currentCalibrationMap.getTag(focusedTag).zPos;
 
-      ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
-      ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
-      ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
+        ImGui::TextWrapped("X Difference: %s (m)",
+                           std::to_string(xDiff).c_str());
+        ImGui::TextWrapped("Y Difference: %s (m)",
+                           std::to_string(yDiff).c_str());
+        ImGui::TextWrapped("Z Difference: %s (m)",
+                           std::to_string(zDiff).c_str());
 
-      ImGui::TextWrapped(
-          "Yaw Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
-              .c_str());
-      ImGui::TextWrapped(
-          "Pitch Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
-              .c_str());
-      ImGui::TextWrapped(
-          "Roll Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
-              .c_str());
+        ImGui::TextWrapped(
+            "Yaw Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
+                .c_str());
+        ImGui::TextWrapped(
+            "Pitch Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
+                .c_str());
+        ImGui::TextWrapped(
+            "Roll Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
+                .c_str());
 
-      ImGui::NewLine();
+        ImGui::NewLine();
 
-      ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
-      ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
-      ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+        ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
+        ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
+        ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+      } else {
+        ImGui::TextWrapped(
+            "Please select tags that are in the ideal field map and "
+            "calibration map");
+      }
     }
 
     if (ImGui::Button("Close")) {
@@ -537,6 +608,78 @@ static void DisplayGui() {
     ImGui::EndPopup();
   }
 
+  if (ImGui::BeginPopupModal("Combine Calibrations", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    openFileButton("Select Ideal Map", selected_field_map, field_map_selector,
+                   "JSON", "*.json");
+    processFileSelector(field_map_selector, selected_field_map);
+    if (!selected_field_map.empty()) {
+      drawCheck();
+      std::ifstream json(selected_field_map);
+      currentReferenceMap = Fieldmap(wpi::json::parse(json));
+      currentCombinerMap = currentReferenceMap;
+    }
+    openFilesButton("Select Field Calibrations",
+                    selected_combination_calibrations,
+                    combination_calibrations_selector, "JSON", "*.json");
+    processFilesSelector(combination_calibrations_selector,
+                         selected_combination_calibrations);
+
+    if (!selected_field_map.empty() &&
+        !selected_combination_calibrations.empty()) {
+      for (std::string& file : selected_combination_calibrations) {
+        ImGui::Selectable(getFileName(file).c_str(), false,
+                          ImGuiSelectableFlags_DontClosePopups);
+        if (ImGui::BeginDragDropSource()) {
+          ImGui::SetDragDropPayload("FieldCalibration", &file, sizeof(file));
+          ImGui::TextUnformatted(file.c_str());
+          ImGui::EndDragDropSource();
+        }
+      }
+
+      for (auto& [key, val] : combiner_map) {
+        EmitEntryTarget(key, val);
+      }
+
+      ImGui::InputInt("Tag ID", &current_combiner_tag_id);
+      ImGui::SameLine();
+      if (ImGui::Button("Add", ImVec2(0, 0)) &&
+          currentCombinerMap.hasTag(current_combiner_tag_id)) {
+        combiner_map.emplace(current_combiner_tag_id, "");
+      }
+      ImGui::SameLine();
+      if (ImGui::Button("Remove", ImVec2(0, 0))) {
+        combiner_map.erase(current_combiner_tag_id);
+      }
+    }
+    ImGui::Separator();
+    if (ImGui::Button("Close", ImVec2(0, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Download", ImVec2(0, 0))) {
+      for (auto& [key, val] : combiner_map) {
+        std::ifstream json(val);
+        Fieldmap map(wpi::json::parse(json));
+        currentCombinerMap.replaceTag(key, map.getTag(key));
+      }
+      field_combination_json = currentCombinerMap.toJson();
+    }
+
+    if (selected_download_directory.empty() &&
+        !field_combination_json.empty() && !download_directory_selector) {
+      download_directory_selector =
+          std::make_unique<pfd::select_folder>("Select Download Folder", "");
+    }
+
+    processDirectorySelector(download_directory_selector,
+                             selected_download_directory);
+    saveCalibration(field_combination_json, selected_download_directory,
+                    "combined_calibration", isCalibrating);
+
+    ImGui::EndPopup();
+  }
+
   ImGui::End();
 }
 

wpical/src/main/native/cpp/fieldcalibration.cpp

@@ -23,7 +23,6 @@
 #include <opencv2/highgui.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/videoio.hpp>
-#include <wpi/json.h>
 
 #include "apriltag.h"
 #include "tag36h11.h"
@@ -433,7 +432,7 @@ inline bool process_video_file(
 }
 
 int fieldcalibration::calibrate(std::string input_dir_path,
-                                std::string output_file_path,
+                                wpi::json& output_json,
                                 std::string camera_model_path,
                                 std::string ideal_map_path, int pinned_tag_id,
                                 bool show_debug_window) {
@@ -605,8 +604,7 @@ int fieldcalibration::calibrate(std::string input_dir_path,
       {"length", static_cast<double>(json.at("field").at("length"))},
       {"width", static_cast<double>(json.at("field").at("width"))}};
 
-  std::ofstream output_file(output_file_path);
-  output_file << observed_map_json.dump(4) << std::endl;
+  output_json = observed_map_json;
 
   return 0;
 }

wpical/src/main/native/cpp/tagpose.cpp

@@ -5,8 +5,10 @@
 #include <tagpose.h>
 
 namespace tag {
-Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
-           double z, double field_length_meters, double field_width_meters) {
+Pose::Pose(int tag_id, double xpos, double ypos, double zpos, double w,
+           double x, double y, double z, double field_length_meters,
+           double field_width_meters) {
+  tagId = tag_id;
   xPos = xpos;
   yPos = ypos;
   zPos = zpos;
@@ -26,4 +28,16 @@ Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
   pitchRot = eulerAngles[1];
   yawRot = eulerAngles[2];
 }
+
+wpi::json Pose::toJson() {
+  return {{"ID", tagId},
+          {"pose",
+           {{"translation", {{"x", xPos}, {"y", yPos}, {"z", zPos}}},
+            {"rotation",
+             {{"quaternion",
+               {{"W", quaternion.w()},
+                {"X", quaternion.x()},
+                {"Y", quaternion.y()},
+                {"Z", quaternion.z()}}}}}}}};
+}
 }  // namespace tag

wpical/src/main/native/include/fieldcalibration.h

@@ -6,10 +6,12 @@
 
 #include <string>
 
+#include <wpi/json.h>
+
 #include "cameracalibration.h"
 
 namespace fieldcalibration {
-int calibrate(std::string input_dir_path, std::string output_file_path,
+int calibrate(std::string input_dir_path, wpi::json& output_json,
               std::string camera_model_path, std::string ideal_map_path,
               int pinned_tag_id, bool show_debug_window);
 }  // namespace fieldcalibration

wpical/src/main/native/include/fieldmap.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include <cmath>
-#include <vector>
+#include <map>
 
 #include <tagpose.h>
 #include <wpi/json.h>
@@ -19,6 +19,7 @@ class Fieldmap {
     double field_width_meters =
         static_cast<double>(json.at("field").at("width"));
     for (const auto& tag : json.at("tags").items()) {
+      double tag_id = static_cast<int>(tag.value().at("ID"));
       double tagXPos =
           static_cast<double>(tag.value().at("pose").at("translation").at("x"));
       double tagYPos =
@@ -34,15 +35,30 @@ class Fieldmap {
       double tagZQuat = static_cast<double>(
           tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
 
-      tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
-                          tagYQuat, tagZQuat, field_length_meters,
-                          field_width_meters);
+      tagMap.emplace(
+          tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
+                            tagXQuat, tagYQuat, tagZQuat, field_length_meters,
+                            field_width_meters));
     }
+    fieldLength = field_length_meters;
+    fieldWidth = field_width_meters;
   }
 
-  const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
+  const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
 
-  int getNumTags() const { return tagVec.size(); }
+  int getNumTags() const { return tagMap.size(); }
+
+  bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
+
+  wpi::json toJson() {
+    wpi::json json;
+    for (auto& [key, val] : tagMap) {
+      json["tags"].push_back(val.toJson());
+    }
+    json["field"]["length"] = fieldLength;
+    json["field"]["width"] = fieldWidth;
+    return json;
+  }
 
   static double minimizeAngle(double angle) {
     angle = std::fmod(angle, 360);
@@ -54,6 +70,13 @@ class Fieldmap {
     return angle;
   }
 
+  void replaceTag(int tag_id, tag::Pose pose) {
+    tagMap.erase(tag_id);
+    tagMap.emplace(tag_id, pose);
+  }
+
  private:
-  std::vector<tag::Pose> tagVec;
+  double fieldLength;
+  double fieldWidth;
+  std::map<int, tag::Pose> tagMap;
 };

wpical/src/main/native/include/tagpose.h

@@ -6,15 +6,19 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <wpi/json.h>
 
 namespace tag {
 class Pose {
  public:
-  Pose(double xpos, double ypos, double zpos, double w, double x, double y,
-       double z, double field_length_meters, double field_width_meters);
+  Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
+       double y, double z, double field_length_meters,
+       double field_width_meters);
+  int tagId;
   double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
   Eigen::Quaterniond quaternion;
   Eigen::Matrix3d rotationMatrix;
   Eigen::Matrix4d transformMatrixFmap;
+  wpi::json toJson();
 };
 }  // namespace tag

wpical/src/test/native/cpp/test_calibrate.cpp

@@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = {
     .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
     .distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
     .avg_reprojection_error = 0.0};
+
+wpi::json output_json;
+
 #ifdef __linux__
 const std::string fileSuffix = ".avi";
 const std::string videoLocation = "/altfieldvideo";
@@ -58,7 +61,7 @@ TEST(Camera_CalibrationTest, MRcal_Atypical) {
 
 TEST(Field_CalibrationTest, Typical) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 0);
@@ -66,7 +69,7 @@ TEST(Field_CalibrationTest, Typical) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       projectRootPath + videoLocation + "/long" + fileSuffix,
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 1);
@@ -74,7 +77,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       calSavePath + "/cameracalibration.json", 3, false);
   EXPECT_EQ(ret, 1);
@@ -82,7 +85,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + "", calSavePath,
+      projectRootPath + "", output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 1);
@@ -90,7 +93,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", 42, false);
   EXPECT_EQ(ret, 1);
@@ -98,7 +101,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
 
 TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
   int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
       calSavePath + "/cameracalibration.json",
       projectRootPath + "/2024-crescendo.json", -1, false);
   EXPECT_EQ(ret, 1);

wpilibNewCommands/src/generate/main/java/commandhid.java.jinja

@@ -107,7 +107,7 @@ public class Command{{ ConsoleName }}Controller extends CommandGenericHID {
 {% endfor -%}
 {% for stick in sticks %}
   /**
-   * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller.
+   * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller. {{ stick.PositiveDirection }} is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generate/main/native/include/frc2/command/button/commandhid.h.jinja

@@ -71,7 +71,7 @@ class Command{{ ConsoleName }}Controller : public CommandGenericHID {
 {% endfor -%}
 {% for stick in sticks %}
   /**
-   * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller.
+   * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller. {{ stick.PositiveDirection }} is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/java/edu/wpi/first/wpilibj2/command/button/CommandPS4Controller.java

@@ -348,7 +348,7 @@ public class CommandPS4Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -357,7 +357,7 @@ public class CommandPS4Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
@@ -366,7 +366,7 @@ public class CommandPS4Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -375,7 +375,7 @@ public class CommandPS4Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/java/edu/wpi/first/wpilibj2/command/button/CommandPS5Controller.java

@@ -348,7 +348,7 @@ public class CommandPS5Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -357,7 +357,7 @@ public class CommandPS5Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
@@ -366,7 +366,7 @@ public class CommandPS5Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -375,7 +375,7 @@ public class CommandPS5Controller extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java

@@ -370,7 +370,7 @@ public class CommandStadiaController extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -379,7 +379,7 @@ public class CommandStadiaController extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -388,7 +388,7 @@ public class CommandStadiaController extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
@@ -397,7 +397,7 @@ public class CommandStadiaController extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.java

@@ -338,7 +338,7 @@ public class CommandXboxController extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -347,7 +347,7 @@ public class CommandXboxController extends CommandGenericHID {
   }
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
@@ -356,7 +356,7 @@ public class CommandXboxController extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
@@ -365,7 +365,7 @@ public class CommandXboxController extends CommandGenericHID {
   }
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/native/include/frc2/command/button/CommandPS4Controller.h

@@ -204,28 +204,28 @@ class CommandPS4Controller : public CommandGenericHID {
                                                 .GetDefaultButtonLoop()) const;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/native/include/frc2/command/button/CommandPS5Controller.h

@@ -204,28 +204,28 @@ class CommandPS5Controller : public CommandGenericHID {
                                                 .GetDefaultButtonLoop()) const;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/native/include/frc2/command/button/CommandStadiaController.h

@@ -216,28 +216,28 @@ class CommandStadiaController : public CommandGenericHID {
                                                 .GetDefaultButtonLoop()) const;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/generated/main/native/include/frc2/command/button/CommandXboxController.h

@@ -190,28 +190,28 @@ class CommandXboxController : public CommandGenericHID {
                                                  .GetDefaultButtonLoop()) const;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return The axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return The axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return The axis value.
    */

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -180,6 +180,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
    * using those requirements have been scheduled as interruptible. If this is the case, they will
    * be interrupted and the command will be scheduled.
    *
+   * <p>WARNING: using this function directly can often lead to unexpected behavior and should be
+   * avoided. Instead Triggers should be used to schedule Commands.
+   *
    * @param command the command to schedule. If null, no-op.
    */
   private void schedule(Command command) {
@@ -230,6 +233,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * Schedules multiple commands for execution. Does nothing for commands already scheduled.
    *
+   * <p>WARNING: using this function directly can often lead to unexpected behavior and should be
+   * avoided. Instead Triggers should be used to schedule Commands.
+   *
    * @param commands the commands to schedule. No-op on null.
    */
   public void schedule(Command... commands) {

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.java

@@ -173,6 +173,9 @@ public class CommandJoystick extends CommandGenericHID {
   /**
    * Get the x position of the HID.
    *
+   * <p>This depends on the mapping of the joystick connected to the current port. On most
+   * joysticks, positive is to the right.
+   *
    * @return the x position
    */
   public double getX() {
@@ -182,6 +185,9 @@ public class CommandJoystick extends CommandGenericHID {
   /**
    * Get the y position of the HID.
    *
+   * <p>This depends on the mapping of the joystick connected to the current port. On most
+   * joysticks, positive is to the back.
+   *
    * @return the y position
    */
   public double getY() {
@@ -218,8 +224,8 @@ public class CommandJoystick extends CommandGenericHID {
   }
 
   /**
-   * Get the magnitude of the direction vector formed by the joystick's current position relative to
-   * its origin.
+   * Get the magnitude of the vector formed by the joystick's current position relative to its
+   * origin.
    *
    * @return The magnitude of the direction vector
    */
@@ -228,16 +234,26 @@ public class CommandJoystick extends CommandGenericHID {
   }
 
   /**
-   * Get the direction of the vector formed by the joystick and its origin in radians.
+   * Get the direction of the vector formed by the joystick and its origin in radians. 0 is forward
+   * and clockwise is positive. (Straight right is π/2.)
    *
    * @return The direction of the vector in radians
    */
   public double getDirectionRadians() {
+    // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system
+    // A positive rotation around the X axis moves the joystick right, and a
+    // positive rotation around the Y axis moves the joystick backward. When
+    // treating them as translations, 0 radians is measured from the right
+    // direction, and angle increases clockwise.
+    //
+    // It's rotated 90 degrees CCW (y is negated and the arguments are reversed)
+    // so that 0 radians is forward.
     return m_hid.getDirectionRadians();
   }
 
   /**
-   * Get the direction of the vector formed by the joystick and its origin in degrees.
+   * Get the direction of the vector formed by the joystick and its origin in degrees. 0 is forward
+   * and clockwise is positive. (Straight right is 90.)
    *
    * @return The direction of the vector in degrees
    */

wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandJoystick.cpp

@@ -26,5 +26,13 @@ double CommandJoystick::GetMagnitude() const {
 }
 
 units::radian_t CommandJoystick::GetDirection() const {
+  // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system
+  // A positive rotation around the X axis moves the joystick right, and a
+  // positive rotation around the Y axis moves the joystick backward. When
+  // treating them as translations, 0 radians is measured from the right
+  // direction, and angle increases clockwise.
+  //
+  // It's rotated 90 degrees CCW (y is negated and the arguments are reversed)
+  // so that 0 radians is forward.
   return m_hid.GetDirection();
 }

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -88,6 +88,10 @@ class CommandScheduler final : public wpi::Sendable,
    * interruptible. If this is the case, they will be interrupted and the
    * command will be scheduled.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param command the command to schedule
    */
   void Schedule(const CommandPtr& command);
@@ -112,6 +116,10 @@ class CommandScheduler final : public wpi::Sendable,
    *
    * The pointer must remain valid through the entire lifecycle of the command.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param command the command to schedule
    */
   void Schedule(Command* command);
@@ -120,6 +128,10 @@ class CommandScheduler final : public wpi::Sendable,
    * Schedules multiple commands for execution. Does nothing for commands
    * already scheduled.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param commands the commands to schedule
    */
   void Schedule(std::span<Command* const> commands);
@@ -128,6 +140,10 @@ class CommandScheduler final : public wpi::Sendable,
    * Schedules multiple commands for execution. Does nothing for commands
    * already scheduled.
    *
+   * @warning Using this function directly can often lead to unexpected behavior
+   * and should be avoided. Instead Triggers should be used to schedule
+   * Commands.
+   *
    * @param commands the commands to schedule
    */
   void Schedule(std::initializer_list<Command*> commands);

wpilibNewCommands/src/main/native/include/frc2/command/button/CommandJoystick.h

@@ -56,7 +56,7 @@ class CommandJoystick : public CommandGenericHID {
   class Trigger Top(frc::EventLoop* loop = CommandScheduler::GetInstance()
                                                .GetDefaultButtonLoop()) const;
   /**
-   * Get the magnitude of the direction vector formed by the joystick's
+   * Get the magnitude of the vector formed by the joystick's
    * current position relative to its origin.
    *
    * @return The magnitude of the direction vector
@@ -64,7 +64,9 @@ class CommandJoystick : public CommandGenericHID {
   double GetMagnitude() const;
 
   /**
-   * Get the direction of the vector formed by the joystick and its origin.
+   * Get the direction of the vector formed by the joystick and its origin. 0 is
+   * forward and clockwise is positive. (Straight right is π/2 radians or 90
+   * degrees.)
    *
    * @return The direction of the vector.
    */

wpilibc/src/generate/main/native/include/frc/hid.h.jinja

@@ -47,7 +47,7 @@ class {{ ConsoleName }}Controller : public GenericHID,
   {{ ConsoleName }}Controller& operator=({{ ConsoleName }}Controller&&) = default;
 {% for stick in sticks %}
   /**
-   * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller.
+   * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller. {{ stick.PositiveDirection }} is positive.
    *
    * @return the axis value.
    */

wpilibc/src/generated/main/native/include/frc/PS4Controller.h

@@ -45,28 +45,28 @@ class PS4Controller : public GenericHID,
   PS4Controller& operator=(PS4Controller&&) = default;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return the axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return the axis value.
    */

wpilibc/src/generated/main/native/include/frc/PS5Controller.h

@@ -45,28 +45,28 @@ class PS5Controller : public GenericHID,
   PS5Controller& operator=(PS5Controller&&) = default;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return the axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return the axis value.
    */

wpilibc/src/generated/main/native/include/frc/StadiaController.h

@@ -45,28 +45,28 @@ class StadiaController : public GenericHID,
   StadiaController& operator=(StadiaController&&) = default;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return the axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return the axis value.
    */

wpilibc/src/generated/main/native/include/frc/XboxController.h

@@ -45,28 +45,28 @@ class XboxController : public GenericHID,
   XboxController& operator=(XboxController&&) = default;
 
   /**
-   * Get the X axis value of left side of the controller.
+   * Get the X axis value of left side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetLeftX() const;
 
   /**
-   * Get the X axis value of right side of the controller.
+   * Get the X axis value of right side of the controller. Right is positive.
    *
    * @return the axis value.
    */
   double GetRightX() const;
 
   /**
-   * Get the Y axis value of left side of the controller.
+   * Get the Y axis value of left side of the controller. Back is positive.
    *
    * @return the axis value.
    */
   double GetLeftY() const;
 
   /**
-   * Get the Y axis value of right side of the controller.
+   * Get the Y axis value of right side of the controller. Back is positive.
    *
    * @return the axis value.
    */

wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp

@@ -18,6 +18,7 @@
 #include <numbers>
 #include <utility>
 
+#include <fmt/format.h>
 #include <hal/HAL.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
@@ -374,7 +375,8 @@ bool ADIS16448_IMU::SwitchToStandardSPI() {
   // Validate the product ID
   uint16_t prod_id = ReadRegister(PROD_ID);
   if (prod_id != 16448) {
-    REPORT_ERROR("Could not find ADIS16448!");
+    REPORT_ERROR(
+        fmt::format("Could not find ADIS16448; got product ID {}", prod_id));
     Close();
     return false;
   }

wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp

@@ -17,6 +17,7 @@
 #include <numbers>
 #include <utility>
 
+#include <fmt/format.h>
 #include <hal/HAL.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
@@ -355,7 +356,8 @@ bool ADIS16470_IMU::SwitchToStandardSPI() {
   // Validate the product ID
   uint16_t prod_id = ReadRegister(PROD_ID);
   if (prod_id != 16982 && prod_id != 16470) {
-    REPORT_ERROR("Could not find ADIS16470!");
+    REPORT_ERROR(
+        fmt::format("Could not find ADIS16470; got product ID {}", prod_id));
     Close();
     return false;
   }

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<HAL_AddressableLEDColorOrder>(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);

wpilibc/src/main/native/cpp/Alert.cpp

@@ -82,17 +82,18 @@ class Alert::SendableAlerts : public nt::NTSendable,
    * @return the SendableAlerts for the group
    */
   static SendableAlerts& ForGroup(std::string_view group) {
-    // Force initialization of SendableRegistry before our magic static to
-    // prevent incorrect destruction order.
-    wpi::SendableRegistry::EnsureInitialized();
-    static wpi::StringMap<Alert::SendableAlerts> groups;
-
-    auto [iter, exists] = groups.try_emplace(group);
-    SendableAlerts& sendable = iter->second;
-    if (!exists) {
-      frc::SmartDashboard::PutData(group, &iter->second);
+    SendableAlerts* salert = nullptr;
+    try {
+      auto* sendable = frc::SmartDashboard::GetData(group);
+      salert = dynamic_cast<SendableAlerts*>(sendable);
+    } catch (frc::RuntimeError&) {
     }
-    return sendable;
+    if (!salert) {
+      // this leaks if ResetSmartDashboardInstance is called, but that's fine
+      salert = new Alert::SendableAlerts;
+      frc::SmartDashboard::PutData(group, salert);
+    }
+    return *salert;
   }
 
  private: