[wpimath] Fix UnscentedKalmanFilter and improve math docs (#7850)

Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular.

This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed.

To fix the bug, a couple things needed to be changed:
all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor,
In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular,
P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively.

Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

.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/sanitizers.yml

@@ -29,11 +29,11 @@ jobs:
             ctest-env: ""
             ctest-flags: ""
     name: "${{ matrix.name }}"
-    runs-on: ubuntu-22.04
-    container: wpilib/roborio-cross-ubuntu:2025-22.04
+    runs-on: ubuntu-24.04
+    container: wpilib/roborio-cross-ubuntu:2025-24.04
     steps:
       - name: Install Dependencies
-        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java clang-14 libprotobuf-dev protobuf-compiler ninja-build
+        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-17 libprotobuf-dev protobuf-compiler ninja-build
 
       - name: Install sccache
         uses: mozilla-actions/sccache-action@v0.0.5
@@ -41,7 +41,7 @@ jobs:
       - uses: actions/checkout@v4
 
       - name: configure
-        run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} ..
+        run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-17 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-17 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} ..
         env:
           SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}

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

CODE_OF_CONDUCT.md

@@ -1,69 +1,47 @@
-# Contributor Covenant Code of Conduct
+# Contributor Community Code of Conduct
 
 ## Our Pledge
 
-We as members, contributors, and leaders pledge to make participation in our
-community a harassment-free experience for everyone, regardless of age, body
-size, visible or invisible disability, ethnicity, sex characteristics, gender
-identity and expression, level of experience, education, socio-economic status,
-nationality, personal appearance, race, religion, or sexual identity
-and orientation.
+As members, contributors, and leaders, we commit to fostering a community where everyone feels safe, respected, and valued. We are dedicated to ensuring that participation in this community is harassment-free, inclusive, and welcoming, regardless of age, body type, abilities (visible or invisible), ethnicity, gender identity or expression, sexual orientation, socioeconomic background, education, nationality, personal appearance, race, or religion.
+
+Above all, we pledge to act with integrity, kindness, and empathy—striving to be not only good participants but also good humans.
 
-We pledge to act and interact in ways that contribute to an open, welcoming,
-diverse, inclusive, and healthy community.
 
 ## Our Standards
 
-Examples of behavior that contributes to a positive environment for our
-community include:
+Positive and respectful behavior is essential to creating a thriving community. This includes:
 
-* Exhibiting Gracious Professionalism® at all times. Gracious Professionalism
+* Practicing **Gracious Professionalism®** at all times. Gracious Professionalism
   is a way of doing things that encourages high-quality work, emphasizes the
   value of others, and respects individuals and the community.
-* Demonstrating empathy and kindness toward other people
-* Being respectful of differing opinions, viewpoints, and experiences
-* Giving and gracefully accepting constructive feedback
-* Accepting responsibility and apologizing to those affected by our mistakes,
-  and learning from the experience
-* Focusing on what is best not just for us as individuals, but for the
-  overall community
+* Showing empathy, kindness, and patience.
+* Respecting diverse perspectives and experiences.
+* Giving and receiving constructive feedback with openness and humility.
+* Owning mistakes, apologizing when necessary, and learning from them.
+* Prioritizing the well-being and success of the entire community over individual interests.
 
-Examples of unacceptable behavior include:
+Unacceptable behavior include:
 
-* The use of sexualized language or imagery, and sexual attention or
-  advances of any kind
+* Using sexualized language, imagery, or making inappropriate advances
 * Trolling, insulting or derogatory comments, and personal or political attacks
-* Public or private harassment
-* Publishing others' private information, such as a physical or email
-  address, without their explicit permission
-* Other conduct which could reasonably be considered inappropriate in a
-  professional setting
+* Harassment in any form, whether public or private.
+* Sharing private information (e.g., email or physical addresses) without explicit consent.
+
+* Any behavior that is unprofessional, harmful, or exclusionary.
 
 ## Enforcement Responsibilities
 
-Community leaders are responsible for clarifying and enforcing our standards of
-acceptable behavior and will take appropriate and fair corrective action in
-response to any behavior that they deem inappropriate, threatening, offensive,
-or harmful.
-
-Community leaders have the right and responsibility to remove, edit, or reject
-comments, commits, code, wiki edits, issues, and other contributions that are
-not aligned to this Code of Conduct, and will communicate reasons for moderation
-decisions when appropriate.
+Community leaders are responsible for maintaining these standards and will take appropriate action to address any behavior deemed harmful, threatening, or inappropriate. Actions may include removing content, issuing warnings, or, when necessary, banning individuals. Moderation decisions will be communicated transparently where appropriate.
 
 ## Scope
 
-This Code of Conduct applies within all community spaces, and also applies when
-an individual is officially representing the community in public spaces.
-Examples of representing our community include using an official e-mail address,
-posting via an official social media account, or acting as an appointed
-representative at an online or offline event.
+This Code of Conduct applies to all community spaces, events, and instances where individuals represent the community (e.g., official email accounts, social media posts, or in-person/virtual events).
 
 ## Enforcement
 
 Instances of abusive, harassing, or otherwise unacceptable behavior may be
 reported to the community leaders responsible for enforcement at
-[conduct@wpilib.org](mailto:conduct@wpilib.org).
+[wpilib@wpi.edu](mailto:wpilib@wpi.edu).
 All complaints will be reviewed and investigated promptly and fairly.
 
 All community leaders are obligated to respect the privacy and security of the
@@ -115,6 +93,9 @@ individual, or aggression toward or disparagement of classes of individuals.
 **Consequence**: A permanent ban from any sort of public interaction within
 the community.
 
+## A Note on Kindness
+Building a community isn’t just about rules—it’s about connection. Every interaction is an opportunity to be understanding, compassionate, and supportive. Being a good human is the key to our ethos.
+
 ## Attribution
 
 This Code of Conduct is adapted from the [Contributor Covenant][homepage],

CONTRIBUTING.md

@@ -80,7 +80,7 @@ xₖ₊₁ = Axₖ + Buₖ
 
 Changes should be submitted as a Pull Request against the main branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current main branch. We do this to make sure that the git history isn't too cluttered.
 
-During the build season, breaking changes or other changes intended for the next season can be created as a pull request against the development branch of WPILib. After the season is over, the changes in the development branch will be merged into main.
+Particularly large and/or breaking changes should be targeted to the 2027 branch, which targets the [SystemCore Robot Controller](https://community.firstinspires.org/introducing-the-future-mobile-robot-controller). The intent is minimize changes for 2026, to allow development to focus on preparing for 2027.
 
 ### Merge Process
 

README.md

@@ -136,6 +136,9 @@ If you have installed the FRC Toolchain to a directory other than the default, o
 
 Once a PR has been submitted, formatting can be run in CI by commenting `/format` on the PR. A new commit will be pushed with the formatting changes.
 
+> [!NOTE]
+> The `/format` action has been temporarily disabled. The individual formatting commands can be run locally as shown below. Alternately, the Lint and Format action for a PR will upload a patch file that can be downloaded and applied manually.
+
 #### wpiformat
 
 wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat` on Windows or `python3 -m wpiformat` on other platforms.

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
@@ -267,12 +269,18 @@ public class AnnotationProcessor extends AbstractProcessor {
       return false;
     }
 
-    processingEnv
-        .getMessager()
-        .printMessage(
-            Diagnostic.Kind.NOTE,
-            "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type",
-            element);
+    var classConfig = element.getEnclosingElement().getAnnotation(Logged.class);
+
+    if (classConfig == null || classConfig.warnForNonLoggableTypes()) {
+      // Not loggable and not explicitly opted out of logging; print a warning message
+      processingEnv
+          .getMessager()
+          .printMessage(
+              Diagnostic.Kind.NOTE,
+              "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type",
+              element);
+    }
+
     return true;
   }
 

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

@@ -71,6 +71,11 @@ public class LoggerGenerator {
         public Naming defaultNaming() {
           return Naming.USE_CODE_NAME;
         }
+
+        @Override
+        public boolean warnForNonLoggableTypes() {
+          return false;
+        }
       };
 
   public LoggerGenerator(ProcessingEnvironment processingEnv, List<ElementHandler> handlers) {

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

@@ -1772,7 +1772,7 @@ class AnnotationProcessorTest {
         """
         package edu.wpi.first.epilogue;
 
-        @Logged
+        @Logged(warnForNonLoggableTypes = true)
         class Example {
           Throwable t;
         }
@@ -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/Logged.java

@@ -124,4 +124,12 @@ public @interface Logged {
    *     for all logged fields and methods in an annotated class
    */
   Naming defaultNaming() default Naming.USE_CODE_NAME;
+
+  /**
+   * Class-level only: if {@link #strategy()} is {@link Strategy#OPT_OUT}, this can be used to quiet
+   * the warnings that are printed for non-loggable fields and methods detected within the class.
+   *
+   * @return true if warnings should be printed, or false if warnings should not be printed
+   */
+  boolean warnForNonLoggableTypes() default false;
 }

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

@@ -224,8 +224,8 @@ class ObjectInfo {
 
 class FieldInfo {
  public:
-  static constexpr auto kDefaultWidth = 16.541052_m;
-  static constexpr auto kDefaultHeight = 8.211_m;
+  static constexpr auto kDefaultWidth = 17.5483_m;
+  static constexpr auto kDefaultHeight = 8.0519_m;
 
   explicit FieldInfo(Storage& storage);
 
@@ -345,7 +345,7 @@ static bool InputPose(frc::Pose2d* pose) {
 }
 
 FieldInfo::FieldInfo(Storage& storage)
-    : m_builtin{storage.GetString("builtin", "2024 Crescendo")},
+    : m_builtin{storage.GetString("builtin", "2025 Reefscape")},
       m_filename{storage.GetString("image")},
       m_width{storage.GetFloat("width", kDefaultWidth.to<float>())},
       m_height{storage.GetFloat("height", kDefaultHeight.to<float>())},
@@ -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

shared/ceres.gradle

@@ -4,7 +4,6 @@ nativeUtils {
             groupId = "edu.wpi.first.thirdparty.frc2024.ceres"
             artifactId = "ceres-cpp"
             headerClassifier = "headers"
-            sourceClassifier = "sources"
             ext = "zip"
             version = '2.2-3'
             targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms)

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/analysis/FeedbackAnalysis.cpp

@@ -8,6 +8,7 @@
 
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
+#include <frc/system/plant/LinearSystemId.h>
 #include <units/acceleration.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
@@ -18,6 +19,7 @@ using namespace sysid;
 
 using Kv_t = decltype(1_V / 1_mps);
 using Ka_t = decltype(1_V / 1_mps_sq);
+using Matrix1d = Eigen::Matrix<double, 1, 1>;
 
 FeedbackGains sysid::CalculatePositionFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
@@ -26,39 +28,32 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
     return {0.0, 0.0};
   }
 
-  // If acceleration requires no effort, velocity becomes an input for position
-  // control. We choose an appropriate model in this case to avoid numerical
+  // If acceleration for position control requires no effort, velocity becomes
+  // an input. We choose an appropriate model in this case to avoid numerical
   // instabilities in the LQR.
-  if (Ka > 1E-7) {
-    // Create a position system from our feedforward gains.
-    frc::LinearSystem<2, 1, 1> system{
-        frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
-        frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
-        frc::Matrixd<1, 1>{0.0}};
-    // Create an LQR with 2 states to control -- position and velocity.
-    frc::LinearQuadraticRegulator<2, 1> controller{
-        system, {params.qp, params.qv}, {params.r}, preset.period};
-    // Compensate for any latency from sensor measurements, filtering, etc.
+  if (std::abs(Ka) < 1e-7) {
+    // System has position state and velocity input
+    frc::LinearSystem<1, 1, 1> system{Matrix1d{0.0}, Matrix1d{1.0},
+                                      Matrix1d{1.0}, Matrix1d{0.0}};
+
+    frc::LinearQuadraticRegulator<1, 1> controller{
+        system, {params.qp}, {params.r}, preset.period};
     controller.LatencyCompensate(system, preset.period,
                                  preset.measurementDelay);
 
-    return {
-        controller.K(0, 0) * preset.outputConversionFactor,
-        controller.K(0, 1) * preset.outputConversionFactor /
-            (preset.normalized ? 1 : units::second_t{preset.period}.value())};
+    return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
   }
 
-  // This is our special model to avoid instabilities in the LQR.
-  auto system = frc::LinearSystem<1, 1, 1>(
-      Eigen::Matrix<double, 1, 1>{0.0}, Eigen::Matrix<double, 1, 1>{1.0},
-      Eigen::Matrix<double, 1, 1>{1.0}, Eigen::Matrix<double, 1, 1>{0.0});
-  // Create an LQR with one state -- position.
-  frc::LinearQuadraticRegulator<1, 1> controller{
-      system, {params.qp}, {params.r}, preset.period};
-  // Compensate for any latency from sensor measurements, filtering, etc.
+  auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
+
+  frc::LinearQuadraticRegulator<2, 1> controller{
+      system, {params.qp, params.qv}, {params.r}, preset.period};
   controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
-  return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
+  return {controller.K(0, 0) * preset.outputConversionFactor,
+          controller.K(0, 1) * preset.outputConversionFactor /
+              (preset.normalized ? 1 : units::second_t{preset.period}.value())};
 }
 
 FeedbackGains sysid::CalculateVelocityFeedbackGains(
@@ -69,20 +64,16 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
   }
 
   // If acceleration for velocity control requires no effort, the feedback
-  // control gains approach zero. We special-case it here because numerical
-  // instabilities arise in LQR otherwise.
-  if (Ka < 1E-7) {
+  // control gains approach zero. We special-case it here to avoid numerical
+  // instabilities in LQR.
+  if (std::abs(Ka) < 1E-7) {
     return {0.0, 0.0};
   }
 
-  // Create a velocity system from our feedforward gains.
-  frc::LinearSystem<1, 1, 1> system{
-      frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
-      frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
-  // Create an LQR controller with 1 state -- velocity.
+  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qv}, {params.r}, preset.period};
-  // Compensate for any latency from sensor measurements, filtering, etc.
   controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
   return {controller.K(0, 0) * preset.outputConversionFactor /

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>
@@ -40,7 +41,7 @@ class InvalidDataError : public std::exception {
    */
   explicit InvalidDataError(std::string_view message) {
     m_message = fmt::format(
-        "{}. Please verify that your units and data is reasonable and then "
+        "{} Please verify that your units and data is reasonable and then "
         "adjust your velocity threshold, test duration, and/or window size to "
         "try to fix this issue.",
         message);
@@ -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();

sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp

@@ -7,7 +7,7 @@
 #include "sysid/analysis/FeedbackAnalysis.h"
 #include "sysid/analysis/FeedbackControllerPreset.h"
 
-TEST(FeedbackAnalysisTest, Velocity1) {
+TEST(FeedbackAnalysisTest, VelocitySystem1) {
   auto Kv = 3.060;
   auto Ka = 0.327;
 
@@ -20,7 +20,7 @@ TEST(FeedbackAnalysisTest, Velocity1) {
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
 
-TEST(FeedbackAnalysisTest, Velocity2) {
+TEST(FeedbackAnalysisTest, VelocitySystem2) {
   auto Kv = 0.0693;
   auto Ka = 0.1170;
 
@@ -33,6 +33,19 @@ TEST(FeedbackAnalysisTest, Velocity2) {
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
 
+TEST(FeedbackAnalysisTest, VelocitySystemWithSmallKa) {
+  auto Kv = 3.060;
+  auto Ka = 0.0;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.00, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
 TEST(FeedbackAnalysisTest, VelocityConversion) {
   auto Kv = 0.0693;
   auto Ka = 0.1170;
@@ -117,6 +130,19 @@ TEST(FeedbackAnalysisTest, Position) {
   EXPECT_NEAR(Kd, 2.48, 0.05);
 }
 
+TEST(FeedbackAnalysisTest, PositionWithSmallKa) {
+  auto Kv = 3.060;
+  auto Ka = 1e-10;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 19.97, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
 TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
   auto Kv = 3.060;
   auto Ka = 0.327;

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