Dont sanitize unique name in calibration JSON HTTP URL (#1846)

## Description

Added paranoia checks to satisfy @Gold872 

## Meta

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [ ] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2024.3.1
- [x] If this PR addresses a bug, a regression test for it is added

.gitattributes

@@ -35,3 +35,6 @@
 *.so binary
 *.dll binary
 *.webp binary
+
+# autogenerated constrained solve pnp code
+photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/generate/**/* linguist-generated

.github/pull_request_template.md

@@ -0,0 +1,17 @@
+## Description
+
+<!-- What changed? Why? (the code + comments should speak for itself on the "how") -->
+
+<!-- Fun screenshots or a cool video or something are super helpful as well. If this touches platform-specific behavior, this is where test evidence should be collected. -->
+
+<!-- Any issues this pull request closes or pull requests this supersedes should be linked with `Closes #issuenumber`. -->
+
+## Meta
+
+Merge checklist:
+- [ ] Pull Request title is [short, imperative summary](https://cbea.ms/git-commit/) of proposed changes
+- [ ] The description documents the _what_ and _why_
+- [ ] If this PR changes behavior or adds a feature, user documentation is updated
+- [ ] If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly
+- [ ] If this PR touches configuration, this is backwards compatible with settings back to v2024.3.1
+- [ ] If this PR addresses a bug, a regression test for it is added

.gitignore

@@ -150,3 +150,4 @@ photon-server/src/main/resources/web/*
 venv
 .venv/*
 .venv
+networktables.json

.styleguide

@@ -21,6 +21,7 @@ modifiableFileExclude {
   \.rknn$
   gradlew
   photon-lib/py/photonlibpy/generated/
+  photon-targeting/src/main/native/cpp/photon/constrained_solvepnp/generate/
   photon-targeting/src/generated/
 }
 

README.md

@@ -1,6 +1,6 @@
 # PhotonVision
 
-[![CI](https://github.com/PhotonVision/photonvision/workflows/CI/badge.svg)](https://github.com/PhotonVision/photonvision/actions?query=workflow%3ACI) [![codecov](https://codecov.io/gh/PhotonVision/photonvision/branch/main/graph/badge.svg)](https://codecov.io/gh/PhotonVision/photonvision) [![Discord](https://img.shields.io/discord/725836368059826228?color=%23738ADB&label=Join%20our%20Discord&logo=discord&logoColor=white)](https://discord.gg/wYxTwym)
+[![Discord](https://img.shields.io/discord/725836368059826228?color=%23738ADB&label=Join%20our%20Discord&logo=discord&logoColor=white)](https://discord.gg/wYxTwym)
 
 PhotonVision is the free, fast, and easy-to-use computer vision solution for the *FIRST* Robotics Competition. You can read an overview of our features [on our website](https://photonvision.org). You can find our comprehensive documentation [here](https://docs.photonvision.org).
 
@@ -42,6 +42,7 @@ Note that these are case sensitive!
     * linuxathena
 - `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to
 - `-Pprofile`: enables JVM profiling
+- `-PwithSanitizers`: On Linux, enables `-fsanitize=address,undefined,leak`
 
 If you're cross-compiling, you'll need the wpilib toolchain installed. This can be done via Gradle: for example `./gradlew installArm64Toolchain` or `./gradlew installRoboRioToolchain`
 

build.gradle

@@ -4,7 +4,7 @@ plugins {
     id "cpp"
     id "com.diffplug.spotless" version "6.24.0"
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
     id 'edu.wpi.first.WpilibTools' version '1.3.0'
     id 'com.google.protobuf' version '0.9.3' apply false
     id 'edu.wpi.first.GradleJni' version '1.1.0'
@@ -33,14 +33,14 @@ ext.allOutputsFolder = file("$project.buildDir/outputs")
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2025.2.1"
+    wpilibVersion = "2025.3.2"
     wpimathVersion = wpilibVersion
     openCVYear = "2025"
     openCVversion = "4.10.0-3"
     joglVersion = "2.4.0"
     javalinVersion = "5.6.2"
     libcameraDriverVersion = "v2025.0.3"
-    rknnVersion = "v2025.0.0"
+    rknnVersion = "dev-v2025.0.0-1-g33b6263"
     frcYear = "2025"
     mrcalVersion = "v2025.0.0";
 

docs/.styleguide

@@ -12,5 +12,6 @@ modifiableFileExclude {
   \.ico$
   \.rknn$
   \.svg$
+  \.woff2$
   gradlew
 }

docs/source/_static/css/pv-icons.css

@@ -1,16 +1,3 @@
-/*!
- *  Font Awesome 4.7.0 by @davegandy - http://fontawesome.io - @fontawesome
- *  License - http://fontawesome.io/license (Font: SIL OFL 1.1, CSS: MIT License)
- */
-
-@font-face {
-    font-family: FontAwesome;
-    src: url(fonts/fontawesome-webfont.eot?674f50d287a8c48dc19ba404d20fe713);
-    src: url(fonts/fontawesome-webfont.eot?674f50d287a8c48dc19ba404d20fe713?#iefix&v=4.7.0) format("embedded-opentype"), url(fonts/fontawesome-webfont.woff2?af7ae505a9eed503f8b8e6982036873e) format("woff2"), url(fonts/fontawesome-webfont.woff?fee66e712a8a08eef5805a46892932ad) format("woff"), url(fonts/fontawesome-webfont.ttf?b06871f281fee6b241d60582ae9369b9) format("truetype"), url(fonts/fontawesome-webfont.svg?912ec66d7572ff821749319396470bde#fontawesomeregular) format("svg");
-    font-weight: 400;
-    font-style:normal
-}
-
 .code-block-caption>.headerlink, dl dt>.headerlink, h1>.headerlink, h2>.headerlink, h3>.headerlink, h4>.headerlink, h5>.headerlink, h6>.headerlink, p.caption>.headerlink, table>caption>.headerlink {
     font-family: FontAwesome;
     font-size: 0.75em;

docs/source/_static/css/v4-font-face.min.css

@@ -0,0 +1,6 @@
+/*!
+ * Font Awesome Free 6.7.2 by @fontawesome - https://fontawesome.com
+ * License - https://fontawesome.com/license/free (Icons: CC BY 4.0, Fonts: SIL OFL 1.1, Code: MIT License)
+ * Copyright 2024 Fonticons, Inc.
+ */
+@font-face{font-family:"FontAwesome";font-display:block;src:url(../webfonts/fa-solid-900.woff2) format("woff2"),url(../webfonts/fa-solid-900.ttf) format("truetype")}@font-face{font-family:"FontAwesome";font-display:block;src:url(../webfonts/fa-brands-400.woff2) format("woff2"),url(../webfonts/fa-brands-400.ttf) format("truetype")}@font-face{font-family:"FontAwesome";font-display:block;src:url(../webfonts/fa-regular-400.woff2) format("woff2"),url(../webfonts/fa-regular-400.ttf) format("truetype");unicode-range:u+f003,u+f006,u+f014,u+f016-f017,u+f01a-f01b,u+f01d,u+f022,u+f03e,u+f044,u+f046,u+f05c-f05d,u+f06e,u+f070,u+f087-f088,u+f08a,u+f094,u+f096-f097,u+f09d,u+f0a0,u+f0a2,u+f0a4-f0a7,u+f0c5,u+f0c7,u+f0e5-f0e6,u+f0eb,u+f0f6-f0f8,u+f10c,u+f114-f115,u+f118-f11a,u+f11c-f11d,u+f133,u+f147,u+f14e,u+f150-f152,u+f185-f186,u+f18e,u+f190-f192,u+f196,u+f1c1-f1c9,u+f1d9,u+f1db,u+f1e3,u+f1ea,u+f1f7,u+f1f9,u+f20a,u+f247-f248,u+f24a,u+f24d,u+f255-f25b,u+f25d,u+f271-f274,u+f278,u+f27b,u+f28c,u+f28e,u+f29c,u+f2b5,u+f2b7,u+f2ba,u+f2bc,u+f2be,u+f2c0-f2c1,u+f2c3,u+f2d0,u+f2d2,u+f2d4,u+f2dc}@font-face{font-family:"FontAwesome";font-display:block;src:url(../webfonts/fa-v4compatibility.woff2) format("woff2"),url(../webfonts/fa-v4compatibility.ttf) format("truetype");unicode-range:u+f041,u+f047,u+f065-f066,u+f07d-f07e,u+f080,u+f08b,u+f08e,u+f090,u+f09a,u+f0ac,u+f0ae,u+f0b2,u+f0d0,u+f0d6,u+f0e4,u+f0ec,u+f10a-f10b,u+f123,u+f13e,u+f148-f149,u+f14c,u+f156,u+f15e,u+f160-f161,u+f163,u+f175-f178,u+f195,u+f1f8,u+f219,u+f27a}

docs/source/conf.py

@@ -79,6 +79,7 @@ source_suffix = [".rst", ".md"]
 
 
 def setup(app):
+    app.add_css_file("css/v4-font-face.min.css")
     app.add_css_file("css/pv-icons.css")
 
 

docs/source/docs/advanced-installation/sw_install/mac-os.md

@@ -23,7 +23,7 @@ Using a JDK other than JDK17 will cause issues when running PhotonVision and is
 Go to the [GitHub releases page](https://github.com/PhotonVision/photonvision/releases) and download the relevant .jar file for your coprocessor.
 
 :::{note}
-If you have an M1/M2 Mac, download the macarm64.jar file.
+If you have an M Series Mac, download the macarm64.jar file.
 
 If you have an Intel based Mac, download the macx64.jar file.
 :::

docs/source/docs/advanced-installation/sw_install/romi.md

@@ -15,12 +15,12 @@ SSH into the Raspberry Pi (using Windows command line, or a tool like [Putty](ht
 :::{attention}
 The version of WPILibPi for the Romi is 2023.2.1, which is not compatible with the current version of PhotonVision. **If you are using WPILibPi 2023.2.1 on your Romi, you must install PhotonVision v2023.4.2 or earlier!**
 
-To install a compatible version of PhotonVision, enter these commands in the SSH terminal connected to the Raspberry Pi. This will download and run the install script, which will intall PhotonVision on your Raspberry Pi and configure it to run at startup.
+To install a compatible version of PhotonVision, enter these commands in the SSH terminal connected to the Raspberry Pi. This will download and run the install script, which will install PhotonVision on your Raspberry Pi and configure it to run at startup.
 
 ```bash
 $ wget https://git.io/JJrEP -O install.sh
 $ sudo chmod +x install.sh
-$ sudo ./install.sh -v 2023.4.2
+$ sudo ./install.sh -v v2023.4.2
 ```
 The install script requires an internet connection, so connecting the Raspberry Pi to an internet-connected router via an Ethernet cable will be the easiest solution. The pi must remain writable while you are following these steps!
 :::

docs/source/docs/advanced-installation/sw_install/windows-pc.md

@@ -12,7 +12,7 @@ Bonjour provides more stable networking when using Windows PCs. Install [Bonjour
 
 ## Installing Java
 
-PhotonVision requires a JDK installed and on the system path. **JDK 17 is needed. Windows Users must use the JDK that ships with WPILib.** [Download and install it from here.](https://github.com/wpilibsuite/allwpilib/releases/tag/v2025.2.1) Either ensure the only Java on your PATH is the WPILIB Java or specify it to gradle with `-Dorg.gradle.java.home=C:\Users\Public\wpilib\2025\jdk`:
+PhotonVision requires a JDK installed and on the system path. **JDK 17 is needed. Windows Users must use the JDK that ships with WPILib.** [Download and install it from here.](https://github.com/wpilibsuite/allwpilib/releases/tag/v2025.3.2) Either ensure the only Java on your PATH is the WPILIB Java or specify it to gradle with `-Dorg.gradle.java.home=C:\Users\Public\wpilib\2025\jdk`:
 
 ```
 > ./gradlew run "-Dorg.gradle.java.home=C:\Users\Public\wpilib\2025\jdk"

docs/source/docs/apriltag-pipelines/3D-tracking.md

@@ -1,10 +1,10 @@
 # 3D Tracking
 
-3D AprilTag tracking will allow you to track the real-world position and rotation of a tag relative to the camera's image sensor. This is useful for robot pose estimation and other applications like autonomous scoring. In order to use 3D tracking, you must first {ref}`calibrate your camera <docs/calibration/calibration:Calibrating Your Camera>`. Once you have, you need to enable 3D mode in the UI and you will now be able to get 3D pose information from the tag! For information on getting and using this information in your code, see {ref}`the programming reference. <docs/programming/index:Programming Reference>`.
+3D AprilTag tracking will allow you to track the real-world position and rotation of a tag relative to the camera's image sensor. This is useful for robot pose estimation and other applications like autonomous scoring. In order to use 3D tracking, you must first {ref}`calibrate your camera <docs/calibration/calibration:Calibrating Your Camera>`. Once you have, you need to enable 3D mode in the UI and you will now be able to get 3D pose information from the tag! For information on getting and using this information in your code, see {ref}`the programming reference <docs/programming/index:Programming Reference>`.
 
 ## Ambiguity
 
-Translating from 2D to 3D using data from the calibration and the four tag corners can lead to "pose ambiguity", where it appears that the AprilTag pose is flipping between two different poses. You can read more about this issue `here. <https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html#d-to-3d-ambiguity>` Ambiguity is calculated as the ratio of reprojection errors between two pose solutions (if they exist), where reprojection error is the error corresponding to the image distance between where the apriltag's corners are detected vs where we expect to see them based on the tag's estimated camera relative pose.
+Translating from 2D to 3D using data from the calibration and the four tag corners can lead to "pose ambiguity", where it appears that the AprilTag pose is flipping between two different poses. You can read more about this issue [here](https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html#d-to-3d-ambiguity). Ambiguity is calculated as the ratio of reprojection errors between two pose solutions (if they exist), where reprojection error is the error corresponding to the image distance between where the apriltag's corners are detected vs where we expect to see them based on the tag's estimated camera relative pose.
 
 There are a few steps you can take to resolve/mitigate this issue:
 

docs/source/docs/apriltag-pipelines/multitag.md

@@ -6,6 +6,10 @@ PhotonVision can combine AprilTag detections from multiple simultaneously observ
 MultiTag requires an accurate field layout JSON to be uploaded! Differences between this layout and the tags' physical location will drive error in the estimated pose output.
 :::
 
+:::{warning}
+For the 2025 Reefscape Season, there are two different field layouts. The first is the [welded field layout](https://github.com/wpilibsuite/allwpilib/blob/main/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.json), which photonvision ships with. The second is the [Andymark field layout](https://github.com/wpilibsuite/allwpilib/blob/main/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.json). It is very important to ensure that you use the correct field layout, both in the [PhotonPoseEstimator](https://docs.photonvision.org/en/latest/docs/programming/photonlib/robot-pose-estimator.html#apriltags-and-photonposeestimator) and on the [coprocessor](https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/multitag.html#updating-the-field-layout).
+:::
+
 ## Enabling MultiTag
 
 Ensure that your camera is calibrated and 3D mode is enabled. Navigate to the Output tab and enable "Do Multi-Target Estimation". This enables MultiTag to use the uploaded field layout JSON to calculate your camera's pose in the field. This 3D transform will be shown as an additional table in the "targets" tab, along with the IDs of AprilTags used to compute this transform.
@@ -51,7 +55,7 @@ The returned field to camera transform is a transform from the fixed field origi
 
 ## Updating the Field Layout
 
-PhotonVision ships by default with the [2025 field layout JSON](https://github.com/wpilibsuite/allwpilib/blob/main/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json). The layout can be inspected by navigating to the settings tab and scrolling down to the "AprilTag Field Layout" card, as shown below.
+PhotonVision ships by default with the [2025 welded field layout JSON](https://github.com/wpilibsuite/allwpilib/blob/main/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.json). The layout can be inspected by navigating to the settings tab and scrolling down to the "AprilTag Field Layout" card, as shown below.
 
 ```{image} images/field-layout.png
 :alt: The currently saved field layout in the Photon UI

docs/source/docs/calibration/calibration.md

@@ -51,6 +51,10 @@ We'll next select a resolution to calibrate and populate our pattern spacing, ma
 :::{warning} Old OpenCV Pattern selector. This should be used in the case that the calibration image is generated from a version of OpenCV before version 4.6.0. This would include targets created by calib.io. If this selector is not set correctly the calibration will be completely invalid. For more info view [this GitHub issue](https://github.com/opencv/opencv_contrib/issues/3291).
 :::
 
+:::{note}
+If you have a [calib.io](https://calib.io/) CharuCo Target you will have to enter the paramaters of your target. For example if your taget says "9x12 | Chceker Size: 30 mm | Marker Size: 22 mm | Dictionary: AruCo DICT 5x5", you would have to set the board type to Dict_5x5_1000, the pattern spacing to 1.1811 in (30 mm converted to inches), the marker size 0.866142 in (22 mm converted to inches), the board width to 12 and the board height to 9. If you chose the wrong tag family the baord wont be detected duting calibration. If you swap the width and height your calibration will have a very high error.
+:::
+
 ### 4. Take at calibration images from various angles.
 
 Now, we'll capture images of our board from various angles. It's important to check that the board overlay matches the board in your image. The further the overdrawn points are from the true position of the chessboard corners, the less accurate the final calibration will be. We'll want to capture enough images to cover the whole camera's FOV (with a minimum of 12). Once we've got our images, we'll click "Finish calibration" and wait for the calibration process to complete. If all goes well, the mean error and FOVs will be shown in the table on the right. The FOV should be close to the camera's specified FOV (usually found in a datasheet) usually within + or - 10 degrees. The mean error should also be low, usually less than 1 pixel.

docs/source/docs/contributing/building-photon.md

@@ -69,6 +69,16 @@ In the root directory:
       ``gradlew buildAndCopyUI``
 ```
 
+### Using hot reload on the UI
+
+In the photon-client directory:
+
+```bash
+npm run dev
+```
+
+This allows you to make UI changes quickly without having to spend time rebuilding the jar. Hot reload is enabled, so changes that you make and save are reflected in the UI immediately. Running this command will give you the URL for accessing the UI, which is on a different port than normal. You must use the printed URL to use hot reload.
+
 ### Build and Run PhotonVision
 
 To compile and run the project, issue the following command in the root directory:

docs/source/docs/contributing/design-descriptions/camera-matching.md

@@ -50,7 +50,7 @@ When a new camera (ie, one we can't match by-path to a deserialized CameraConfig
 
 ## Startup:
 
-- GIVEN An emtpy set of deserialized Camera Configurations
+- GIVEN An empty set of deserialized Camera Configurations
 <br>WHEN PhotonVision starts
 <br>THEN no VisionModules will be started
 
@@ -72,12 +72,12 @@ When a new camera (ie, one we can't match by-path to a deserialized CameraConfig
 
 ## Camera (re)enumeration:
 
-- GIVEN a NEW USB CAMERA is avaliable for enumeration
+- GIVEN a NEW USB CAMERA is available for enumeration
 <br>WHEN a USB camera is discovered by VisionSourceManager
 <br>AND the USB camera's VIDEO DEVICE PATH is not in the set of DESERIALIZED CAMERA CONFIGURATIONS
 <br>THEN a UNIQUE NAME will be assigned to the camera info
 
-- GIVEN a NEW USB CAMERA is avaliable for enumeration
+- GIVEN a NEW USB CAMERA is available for enumeration
 <br>WHEN a USB camera is discovered by VisionSourceManager
 <br>AND the USB camera's VIDEO DEVICE PATH is in the set of DESERIALIZED CAMERA CONFIGURATIONS
 <br>THEN a UNIQUE NAME equal to the matching DESERIALIZED CAMERA CONFIGURATION will be assigned to the camera info
@@ -86,13 +86,13 @@ When a new camera (ie, one we can't match by-path to a deserialized CameraConfig
 ## Creating from a new camera
 
 - Given: A UNIQUE NAME from a NEW USB CAMERA
-<br>WHEN I request a new VisionModule is created for this NEW USB CAMREA
+<br>WHEN I request a new VisionModule is created for this NEW USB CAMERA
 <br>AND the camera has a VALID USB PATH
 <br>AND the camera's VALID USB PATH is not in use by any CURRENTLY ACTIVE CAMERAS
 <br>THEN a NEW VisionModule will be started for the NEW USB CAMERA using the VALID USB PATH
 
 - Given: A UNIQUE NAME from a NEW USB CAMERA
-<br>WHEN I request a new VisionModule is created for this NEW USB CAMREA
+<br>WHEN I request a new VisionModule is created for this NEW USB CAMERA
 <br>AND the camera does not have a VALID USB PATH
 <br>AND the camera's VIDEO DEVICE PATH is not in use by any CURRENTLY ACTIVE CAMERAS
 <br>THEN a NEW VisionModule will be started for the NEW USB CAMERA using the VIDEO DEVICE PATH

docs/source/docs/contributing/design-descriptions/e2e-latency.md

@@ -3,7 +3,7 @@
 
 ## A primer on time
 
-Expecially starting around 2022 with AprilTags making localization easier, providing a way to know when a camera image was captured at became more important for localization.
+Especially starting around 2022 with AprilTags making localization easier, providing a way to know when a camera image was captured at became more important for localization.
 Since the [creation of USBFrameProvider](https://github.com/PhotonVision/photonvision/commit/f92bf670ded52b59a00352a4a49c277f01bae305), we used the time [provided by CSCore](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/cscore/CvSink.html#grabFrame(org.opencv.core.Mat)) to tell when a camera image was captured at, but just keeping track of "CSCore told us frame N was captured 104.21s after the Raspberry Pi turned on" isn't very helpful. We can decompose this into asking:
 
 - At what time was a particular image captured at, in the coprocessor's timebase?
@@ -29,13 +29,13 @@ WPILib's CSCore is a platform-agnostic wrapper around Windows, Linux, and MacOS
 
 Prior to https://github.com/wpilibsuite/allwpilib/pull/7609, CSCore used the [time it dequeued the buffer at](https://github.com/wpilibsuite/allwpilib/blob/17a03514bad6de195639634b3d57d5ac411d601e/cscore/src/main/native/linux/UsbCameraImpl.cpp#L559) as the image capture time. But this doesn't account for exposure time or latency introduced by the camera + USB stack + Linux itself.
 
-V4L does expose (with some [very heavy caviets](https://github.com/torvalds/linux/blob/fc033cf25e612e840e545f8d5ad2edd6ba613ed5/drivers/media/usb/uvc/uvc_video.c#L600) for some troublesome cameras) its best guess at the time an image was captured at via [buffer flags](https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/buffer.html#buffer-flags). In my testing, all my cameras were able to provide timestamps with both these flags set:
+V4L does expose (with some [very heavy caveats](https://github.com/torvalds/linux/blob/fc033cf25e612e840e545f8d5ad2edd6ba613ed5/drivers/media/usb/uvc/uvc_video.c#L600) for some troublesome cameras) its best guess at the time an image was captured at via [buffer flags](https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/buffer.html#buffer-flags). In my testing, all my cameras were able to provide timestamps with both these flags set:
 - `V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC`: The buffer timestamp has been taken from the CLOCK_MONOTONIC clock [...] accessible via `clock_gettime()`.
 - `V4L2_BUF_FLAG_TSTAMP_SRC_SOE`: Start Of Exposure. The buffer timestamp has been taken when the exposure of the frame has begun.
 
 I'm sure that we'll find a camera that doesn't play nice, because we can't have nice things :). But until then, using this timestamp gets us a free accuracy bump.
 
-Other things to note: This gets us an estimate at when the camera *started* collecting photons. The camera's sensor will remain collecitng light for up to the total integration time, plus readout time for rolling shutter cameras.
+Other things to note: This gets us an estimate at when the camera *started* collecting photons. The camera's sensor will remain collecting light for up to the total integration time, plus readout time for rolling shutter cameras.
 
 ## Latency Testing
 
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
 ```
 </details>
 
-I've decreased camera exposure as much as possible (so we know with reasonable confidence that the image was collected right at the start of the exposure time reported by V4L), but we only get back new images at 60fps. So we don't know when between frame N and N+1 the LED turned on - just that somtime between now and 1/60th of a second a go, the LED turned on.
+I've decreased camera exposure as much as possible (so we know with reasonable confidence that the image was collected right at the start of the exposure time reported by V4L), but we only get back new images at 60fps. So we don't know when between frame N and N+1 the LED turned on - just that sometime between now and 1/60th of a second a go, the LED turned on.
 
 The test coprocessor was an Orange Pi 5 running a PhotonVision 2025 (Ubuntu 24.04 based) image, with an ArduCam OV9782 at 1280x800, 60fps, MJPG running a reflective pipeline.
 
@@ -133,4 +133,4 @@ With the camera capturing at 60fps, the time between successive frames is only ~
 
 ### Future Work
 
-This test also makes no effort to isolate error from time syncronization from error introduced by frame time measurement - we're just interested in overall error. Future work could investigate the latency contribution
+This test also makes no effort to isolate error from time synchronization from error introduced by frame time measurement - we're just interested in overall error. Future work could investigate the latency contribution

docs/source/docs/contributing/design-descriptions/time-sync.md

@@ -76,7 +76,7 @@ Communication between server and clients shall occur over the User Datagram Prot
 
 ## Message Format
 
-The message format forgoes CRCs (as these are provided by the Ethernet physical layer) or packet delimination (as our packetsa are assumed be under the network MTU). **TSP Ping** and **TSP Pong** messages shall be encoded in a manor compatible with a WPILib packed struct with respect to byte alignment and endienness.
+The message format forgoes CRCs (as these are provided by the Ethernet physical layer) or packet delineation (as our packets are assumed be under the network MTU). **TSP Ping** and **TSP Pong** messages shall be encoded in a manor compatible with a WPILib packed struct with respect to byte alignment and endianness.
 
 ### TSP Ping
 
@@ -98,7 +98,7 @@ The message format forgoes CRCs (as these are provided by the Ethernet physical
 
 ## Optional Protocol Extensions
 
-Clients may publish statistics to NetworkTables. If they do, they shall publish to a key that is globally unique per participant in the Time Synronization network. If a client implements this, it shall provide the following publishers:
+Clients may publish statistics to NetworkTables. If they do, they shall publish to a key that is globally unique per participant in the Time Synchronization network. If a client implements this, it shall provide the following publishers:
 
 | Key | Type | Notes |
 | ------ | ------ | ---- |

docs/source/docs/description.md

@@ -27,7 +27,7 @@ Using PhotonVision allows the user to calibrate for their specific camera, which
 
 ### Low Latency, High FPS Processing
 
-PhotonVision exposes specalized hardware on select coprocessors to maximize processing speed. This allows for lower-latency detection of targets to ensure you aren't losing out on any performance.
+PhotonVision exposes specialized hardware on select coprocessors to maximize processing speed. This allows for lower-latency detection of targets to ensure you aren't losing out on any performance.
 
 ### Fully Open Source and Active Developer Community
 

docs/source/docs/examples/aimingatatarget.md

@@ -5,8 +5,8 @@ The following example is from the PhotonLib example repository ([Java](https://g
 ## Knowledge and Equipment Needed
 
 - A Robot
-- A camera mounted rigidly to the robot's frame, cenetered and pointed forward.
-- A coprocessor running PhotonVision with an AprilTag or Aurco 2D Pipeline.
+- A camera mounted rigidly to the robot's frame, centered and pointed forward.
+- A coprocessor running PhotonVision with an AprilTag or Aruco 2D Pipeline.
 - [A printout of AprilTag 7](https://firstfrc.blob.core.windows.net/frc2025/FieldAssets/Apriltag_Images_and_User_Guide.pdf), mounted on a rigid and flat surface.
 
 ## Code

docs/source/docs/objectDetection/about-object-detection.md

@@ -2,15 +2,15 @@
 
 ## How does it work?
 
-PhotonVision supports object detection using neural network accelerator hardware built into Orange Pi 5/5+ coprocessors. The Neural Processing Unit, or NPU, is [used by PhotonVision](https://github.com/PhotonVision/rknn_jni/tree/main) to massively accelerate certain math operations like those needed for running ML-based object detection.
+PhotonVision supports object detection using neural network accelerator hardware built into Orange Pi 5/5+ coprocessors. Please note that the Orange Pi 5/5+ are the only coprocessors that are currently supported. The Neural Processing Unit, or NPU, is [used by PhotonVision](https://github.com/PhotonVision/rknn_jni/tree/main) to massively accelerate certain math operations like those needed for running ML-based object detection.
 
-For the 2025 season, PhotonVision does not currently ship with a pre-trained detector. If teams are interested in using object detection, they can follow the custom process outlined {ref}`below <docs/objectDetection/about-object-detection:Uploading Custom Models>`.
+For the 2025 season, PhotonVision ships with a pretrained ALGAE model. A model to detect coral is not currently stable, and interested teams should ask in the Photonvision discord.
 
 ## Tracking Objects
 
 Before you get started with object detection, ensure that you have followed the previous sections on installation, wiring, and networking. Next, open the Web UI, go to the top right card, and switch to the “Object Detection” type. You should see a screen similar to the image above.
 
-PhotonVision does not currently ship with a pretrained model. Models are trained to detect one or more object "classes" (such as cars, stoplights) in an input image. For each detected object, the model outputs a bounding box around where in the image the object is located, what class the object belongs to, and a unitless confidence between 0 and 1.
+Models are trained to detect one or more object "classes" (such as cars, stoplights) in an input image. For each detected object, the model outputs a bounding box around where in the image the object is located, what class the object belongs to, and a unitless confidence between 0 and 1.
 
 :::{note}
 This model output means that while its fairly easy to say that "this rectangle probably contains an object", we don't have any information about the object's orientation or location. Further math in user code would be required to make estimates about where an object is physically located relative to the camera.
@@ -35,16 +35,21 @@ Photonvision will letterbox your camera frame to 640x640. This means that if you
 
 ## Training Custom Models
 
-Coming soon!
+:::{warning}
+Power users only. This requires some setup, such as obtaining your own dataset and installing various tools. It's additionally advised to have a general knowledge of ML before attempting to train your own model. Additionally, this is not officially supported by Photonvision, and any problems that may arise are not attributable to Photonvision.
+:::
+
+Before beginning, it is necessary to install the [rknn-toolkit2](https://github.com/airockchip/rknn-toolkit2). Then, install the relevant [Ultralytics repository](https://github.com/airockchip?tab=repositories&q=yolo&type=&language=&sort=) from this list. After training your model, export it to `rknn`. This will give you an `onnx` file, formatted for conversion. Copy this file to the relevant folder in [rknn_model_zoo](https://github.com/airockchip/rknn_model_zoo), and use the conversion script located there to convert it. If necessary, modify the script to provide the path to your training database for quantization.
 
 ## Uploading Custom Models
 
 :::{warning}
-PhotonVision currently ONLY supports 640x640 YOLOv5 & YOLOv8 models trained and converted to `.rknn` format for RK3588 CPUs! Other models require different post-processing code and will NOT work. The model conversion process is also highly particular. Proceed with care.
+PhotonVision currently ONLY supports 640x640 Ultralytics YOLOv5, YOLOv8, and YOLOv11 models trained and converted to `.rknn` format for RK3588 CPUs! Other models require different post-processing code and will NOT work. The model conversion process is also highly particular. Proceed with care.
 :::
 
 In the settings, under `Device Control`, there's an option to upload a new object detection model. Naming convention
-should be `name-verticalResolution-horizontalResolution-modelType`. Additionally, the labels
-file ought to have the same name as the RKNN file, with `-labels` appended to the end. For example, if the
-RKNN file is named `note-640-640-yolov5s.rknn`, the labels file should be named
-`note-640-640-yolov5s-labels.txt`.
+should be `name-verticalResolution-horizontalResolution-yolovXXX`. The
+`name` should only include alphanumeric characters, periods, and underscores. Additionally, the labels
+file ought to have the same name as the RKNN file, with `-labels` appended to the end. For
+example, if the RKNN file is named `Algae_1.03.2025-640-640-yolov5s.rknn`, the labels file should be
+named `Algae_1.03.2025-640-640-yolov5s-labels.txt`.

docs/source/docs/programming/photonlib/robot-pose-estimator.md

@@ -32,7 +32,7 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all
 
 ## Creating a `PhotonPoseEstimator`
 
-The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, `PhotonCamera`, and `Transform3d`. `PoseStrategy` has six possible values:
+The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, `PhotonCamera`, and `Transform3d`. `PoseStrategy` has nine possible values:
 
 - MULTI_TAG_PNP_ON_COPROCESSOR
     - Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate.
@@ -47,6 +47,19 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s
     - Choose the Pose which is closest to the last pose calculated.
 - AVERAGE_BEST_TARGETS
     - Choose the Pose which is the average of all the poses from each tag.
+- MULTI_TAG_PNP_ON_RIO
+    - A slower, older version of MULTI_TAG_PNP_ON_COPROCESSOR, not recommended for use.
+- PNP_DISTANCE_TRIG_SOLVE
+    - Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order
+      to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading
+      data is up-to-date. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98).
+- CONSTRAINED_SOLVEPNP
+    - Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase
+      flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms.
+      This also requires addHeadingData to be called every frame so heading data is up to date.
+      If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to
+      the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the
+      seed.
 
 ```{eval-rst}
 .. tab-set-code::
@@ -142,3 +155,7 @@ Updates the stored reference pose when using the CLOSEST_TO_REFERENCE_POSE strat
 ### `setLastPose(Pose3d lastPose)`
 
 Update the stored last pose. Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy.
+
+### `addHeadingData(double timestampSeconds, Rotation2d heading)`
+
+Adds robot heading data to be stored in buffer. Must be called periodically with a proper timestamp for the PNP_DISTANCE_TRIG_SOLVE and CONSTRAINED_SOLVEPNP strategies

docs/source/docs/programming/photonlib/using-target-data.md

@@ -106,7 +106,7 @@ You can get a [translation](https://docs.wpilib.org/en/latest/docs/software/adva
    .. code-block:: C++
 
       // Calculate a translation from the camera to the target.
-      frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslationn(
+      frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslation(
         distance, frc::Rotation2d(units::degree_t(-target.GetYaw())));
 
    .. code-block:: Python

docs/source/docs/quick-start/arducam-cameras.md

@@ -17,6 +17,6 @@ Arducam cameras are supported for setups with multiple devices. This is possible
 3. **Save Settings**: Ensure that you save the settings after selecting the appropriate camera model for each device.
 
 ```{image} images/setArducamModel.png
-:alt: The camera model can be selected from the Arudcam model selector in the cameras tab
+:alt: The camera model can be selected from the Arducam model selector in the cameras tab
 :align: center
 ```

docs/source/docs/quick-start/networking.md

@@ -11,7 +11,7 @@ When using PhotonVision off robot, you _MUST_ plug the coprocessor into a physic
 :::{tab-item} New Radio (2025 - present)
 
 ```{danger}
-Ensure that the radio's DIP switches 1 and 2 are turned off; otherwise, the radio PoE feature may electrically destroy your coprocessor. [More info.](https://frc-radio.vivid-hosting.net/getting-started/passive-power-over-ethernet-poe-for-downstream-devices)
+Ensure that the radio's DIP switches 1 and 2 are turned off; otherwise, the radio PoE feature may electrically destroy your coprocessor. [More info.](https://frc-radio.vivid-hosting.net/overview/wiring-your-radio#power-over-ethernet-poe-for-downstream-devices)
 ```
 
 ```{image} images/networking-diagram-vividhosting.png
@@ -91,3 +91,7 @@ The address in the code above (`photonvision.local`) is the hostname of the copr
 ## Camera Stream Ports
 
 The camera streams start at 1181 with two ports for each camera (ex. 1181 and 1182 for camera one, 1183 and 1184 for camera two, etc.). The easiest way to identify the port of the camera that you want is by double clicking on the stream, which opens it in a separate page. The port will be listed below the stream.
+
+:::{warning}
+If your camera stream isn't sent to the same port as it's originally found on, its stream will not be visible in the UI.
+:::

photon-client/src/components/app/photon-camera-stream.vue

@@ -1,6 +1,7 @@
 <script setup lang="ts">
 import { computed, inject, ref, onBeforeUnmount } from "vue";
 import { useStateStore } from "@/stores/StateStore";
+import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import loadingImage from "@/assets/images/loading-transparent.svg";
 import type { StyleValue } from "vue/types/jsx";
 import PvIcon from "@/components/common/pv-icon.vue";
@@ -58,9 +59,9 @@ const overlayStyle = computed<StyleValue>(() => {
 
 const handleCaptureClick = () => {
   if (props.streamType === "Raw") {
-    props.cameraSettings.pipelineSettings[props.cameraSettings.currentPipelineIndex].saveInputSnapshot();
+    useCameraSettingsStore().saveInputSnapshot();
   } else {
-    props.cameraSettings.pipelineSettings[props.cameraSettings.currentPipelineIndex].saveOutputSnapshot();
+    useCameraSettingsStore().saveOutputSnapshot();
   }
 };
 const handlePopoutClick = () => {

photon-client/src/components/dashboard/tabs/InputTab.vue

@@ -170,5 +170,13 @@ const interactiveCols = computed(() =>
       :select-cols="interactiveCols"
       @input="(args) => handleStreamResolutionChange(args)"
     />
+    <pv-switch
+      v-if="useCameraSettingsStore().isDriverMode"
+      v-model="useCameraSettingsStore().currentPipelineSettings.crosshair"
+      label="Crosshair"
+      :switch-cols="interactiveCols"
+      tooltip="Enables or disables a crosshair overlay on the camera stream"
+      @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ crosshair: args }, false)"
+    />
   </div>
 </template>

photon-client/src/components/dashboard/tabs/PnPTab.vue

@@ -26,7 +26,8 @@ const interactiveCols = computed(() =>
         { name: '2020 Power Cell (7in)', value: TargetModel.CircularPowerCell7in },
         { name: '2022 Cargo Ball (9.5in)', value: TargetModel.RapidReactCircularCargoBall },
         { name: '2023 AprilTag 6in (16h5)', value: TargetModel.AprilTag6in_16h5 },
-        { name: '2024 AprilTag 6.5in (36h11)', value: TargetModel.AprilTag6p5in_36h11 }
+        { name: '2024 AprilTag 6.5in (36h11)', value: TargetModel.AprilTag6p5in_36h11 },
+        { name: '2025 Algae (16.25in)', value: TargetModel.ReefscapeAlgae }
       ]"
       :select-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ targetModel: value }, false)"

photon-client/src/components/dashboard/tabs/TargetsTab.vue

@@ -108,8 +108,8 @@ const resetCurrentBuffer = () => {
                 <td class="text-center">{{ target.area.toFixed(2) }}&deg;</td>
               </template>
               <template v-else>
-                <td class="text-center">{{ target.pose?.x.toFixed(2) }}&nbsp;m</td>
-                <td class="text-center">{{ target.pose?.y.toFixed(2) }}&nbsp;m</td>
+                <td class="text-center">{{ target.pose?.x.toFixed(3) }}&nbsp;m</td>
+                <td class="text-center">{{ target.pose?.y.toFixed(3) }}&nbsp;m</td>
                 <td class="text-center">{{ toDeg(target.pose?.angle_z || 0).toFixed(2) }}&deg;</td>
               </template>
               <template
@@ -157,13 +157,13 @@ const resetCurrentBuffer = () => {
             <tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
               <tr>
                 <td class="text-center white--text">
-                  {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }}&nbsp;m
+                  {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(3) }}&nbsp;m
                 </td>
                 <td class="text-center white--text">
-                  {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }}&nbsp;m
+                  {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(3) }}&nbsp;m
                 </td>
                 <td class="text-center white--text">
-                  {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(2) }}&nbsp;m
+                  {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.z.toFixed(3) }}&nbsp;m
                 </td>
                 <td class="text-center white--text">
                   {{

photon-client/src/components/settings/ObjectDetectionCard.vue

@@ -88,11 +88,12 @@ const supportedModels = computed(() => {
               <v-card-title>Import New Object Detection Model</v-card-title>
               <v-card-text>
                 Upload a new object detection model to this device that can be used in a pipeline. Naming convention
-                should be <code>name-verticalResolution-horizontalResolution-modelType</code>. Additionally, the labels
-                file ought to have the same name as the RKNN file, with <code>-labels</code> appended to the end. For
-                example, if the RKNN file is named <code>note-640-640-yolov5s.rknn</code>, the labels file should be
-                named <code>note-640-640-yolov5s-labels.txt</code>. Note that ONLY 640x640 YOLOv5 & YOLOv8 models
-                trained and converted to `.rknn` format for RK3588 CPUs are currently supported!
+                should be <code>name-verticalResolution-horizontalResolution-yolovXXX</code>. The
+                <code>name</code> should only include alphanumeric characters, periods, and underscores. Additionally,
+                the labels file ought to have the same name as the RKNN file, with <code>-labels</code> appended to the
+                end. For example, if the RKNN file is named <code>note-640-640-yolov5s.rknn</code>, the labels file
+                should be named <code>note-640-640-yolov5s-labels.txt</code>. Note that ONLY 640x640 YOLOv5, YOLOv8, and
+                YOLOv11 models trained and converted to `.rknn` format for RK3588 CPUs are currently supported!
                 <v-row class="mt-6 ml-4 mr-8">
                   <v-file-input v-model="importRKNNFile" label="RKNN File" accept=".rknn" />
                 </v-row>

photon-client/src/stores/settings/CameraSettingsStore.ts

@@ -478,7 +478,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
       const url = new URL(`http://${host}/api/utils/getCalibrationJSON`);
       url.searchParams.set("width", Math.round(resolution.width).toFixed(0));
       url.searchParams.set("height", Math.round(resolution.height).toFixed(0));
-      url.searchParams.set("cameraUniqueName", cameraUniqueName.replace(" ", "").trim().toLowerCase());
+      url.searchParams.set("cameraUniqueName", cameraUniqueName);
 
       return url.href;
     }

photon-client/src/types/PipelineTypes.ts

@@ -27,7 +27,8 @@ export enum TargetModel {
   CircularPowerCell7in = 3,
   RapidReactCircularCargoBall = 4,
   AprilTag6in_16h5 = 5,
-  AprilTag6p5in_36h11 = 6
+  AprilTag6p5in_36h11 = 6,
+  ReefscapeAlgae = 7
 }
 
 export interface PipelineSettings {

photon-core/src/main/java/org/photonvision/common/configuration/DatabaseSchema.java

@@ -19,7 +19,7 @@ package org.photonvision.common.configuration;
 
 /**
  * Add migrations by adding the SQL commands for each migration sequentially to this array. DO NOT
- * edit or delete existing SQL commands. That will lead to producing an icompatible database.
+ * edit or delete existing SQL commands. That will lead to producing an incompatible database.
  *
  * <p>You can use multiple SQL statements in one migration step as long as you separate them with a
  * semicolon (;).

photon-core/src/main/java/org/photonvision/common/configuration/NeuralNetworkModelManager.java

@@ -33,6 +33,8 @@ import java.util.Map;
 import java.util.Optional;
 import java.util.jar.JarEntry;
 import java.util.jar.JarFile;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -301,4 +303,66 @@ public class NeuralNetworkModelManager {
             logger.error("Error extracting models", e);
         }
     }
+
+    private static Pattern modelPattern =
+            Pattern.compile("^([a-zA-Z0-9._]+)-(\\d+)-(\\d+)-(yolov(?:5|8|11)[nsmlx]*)\\.rknn$");
+
+    private static Pattern labelsPattern =
+            Pattern.compile("^([a-zA-Z0-9._]+)-(\\d+)-(\\d+)-(yolov(?:5|8|11)[nsmlx]*)-labels\\.txt$");
+
+    /**
+     * Check naming conventions for models and labels.
+     *
+     * <p>This is static as it is not dependent on the state of the class.
+     *
+     * @param modelName the name of the model
+     * @param labelsName the name of the labels file
+     * @throws IllegalArgumentException if the names are invalid
+     */
+    public static void verifyRKNNNames(String modelName, String labelsName) {
+        // check null
+        if (modelName == null || labelsName == null) {
+            throw new IllegalArgumentException("Model name and labels name cannot be null");
+        }
+
+        // These patterns check that the naming convention of
+        // name-widthResolution-heightResolution-modelType is followed
+
+        Matcher modelMatcher = modelPattern.matcher(modelName);
+        Matcher labelsMatcher = labelsPattern.matcher(labelsName);
+
+        if (!modelMatcher.matches() || !labelsMatcher.matches()) {
+            throw new IllegalArgumentException(
+                    "Model name and labels name must follow the naming convention of name-widthResolution-heightResolution-modelType.rknn and name-widthResolution-heightResolution-modelType-labels.txt");
+        }
+
+        if (!modelMatcher.group(1).equals(labelsMatcher.group(1))
+                || !modelMatcher.group(2).equals(labelsMatcher.group(2))
+                || !modelMatcher.group(3).equals(labelsMatcher.group(3))
+                || !modelMatcher.group(4).equals(labelsMatcher.group(4))) {
+            throw new IllegalArgumentException("Model name and labels name must be matching.");
+        }
+    }
+
+    /**
+     * Parse RKNN name and return the name, width, height, and model type.
+     *
+     * <p>This is static as it is not dependent on the state of the class.
+     *
+     * @param modelName the name of the model
+     * @throws IllegalArgumentException if the model name does not follow the naming convention
+     * @return an array containing the name, width, height, and model type
+     */
+    public static String[] parseRKNNName(String modelName) {
+        Matcher modelMatcher = modelPattern.matcher(modelName);
+
+        if (!modelMatcher.matches()) {
+            throw new IllegalArgumentException(
+                    "Model name must follow the naming convention of name-widthResolution-heightResolution-modelType.rknn");
+        }
+
+        return new String[] {
+            modelMatcher.group(1), modelMatcher.group(2), modelMatcher.group(3), modelMatcher.group(4)
+        };
+    }
 }

photon-core/src/main/java/org/photonvision/common/hardware/OsImageVersion.java

@@ -28,7 +28,7 @@ import org.photonvision.common.logging.Logger;
  * Our blessed images inject the current version via this build workflow:
  * https://github.com/PhotonVision/photon-image-modifier/blob/2e5ddb6b599df0be921c12c8dbe7b939ecd7f615/.github/workflows/main.yml#L67
  *
- * <p>This class provides a convienent abstraction around this
+ * <p>This class provides a convenient abstraction around this
  */
 public class OsImageVersion {
     private static final Logger logger = new Logger(OsImageVersion.class, LogGroup.General);

photon-core/src/main/java/org/photonvision/common/util/ShellExec.java

@@ -145,8 +145,9 @@ public class ShellExec {
         exitCode = 0;
         if (wait) {
             try {
-                process.waitFor();
-                exitCode = process.exitValue();
+                exitCode = process.waitFor();
+                errorGobbler.join();
+                outputGobbler.join();
             } catch (InterruptedException ignored) {
             }
         }

photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java

@@ -31,7 +31,7 @@ public enum CameraQuirk {
     /** Separate red/blue gain controls available */
     @JsonAlias("AWBGain") // remove after https://github.com/PhotonVision/photonvision/issues/1488
     AwbRedBlueGain,
-    /** Will not work with photonvision - Logitec C270 at least */
+    /** Will not work with photonvision - Logitech C270 at least */
     CompletelyBroken,
     /** Has adjustable focus and autofocus switch */
     AdjustableFocus,

photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java

@@ -31,9 +31,9 @@ public class QuirkyCamera {
                     // SeeCam, which has an odd exposure range
                     new QuirkyCamera(
                             0x2560, 0xc128, "See3Cam_24CUG", CameraQuirk.Gain, CameraQuirk.See3Cam_24CUG),
-                    // Chris's older generic "Logitec HD Webcam"
+                    // Chris's older generic "Logitech HD Webcam"
                     new QuirkyCamera(0x9331, 0x5A3, CameraQuirk.CompletelyBroken),
-                    // Logitec C270
+                    // Logitech C270
                     new QuirkyCamera(0x825, 0x46D, CameraQuirk.CompletelyBroken),
                     // A laptop internal camera someone found broken
                     new QuirkyCamera(0x0bda, 0x5510, CameraQuirk.CompletelyBroken),

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/USBCameraSource.java

@@ -180,7 +180,7 @@ public class USBCameraSource extends VisionSource {
         // And update the settables' FrameStaticProps
         settables.setVideoMode(oldVideoMode);
 
-        // Propogate our updated settables over to the frame provider
+        // Propagate our updated settables over to the frame provider
         ((USBFrameProvider) this.usbFrameProvider).updateSettables(this.settables);
     }
 

photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java

@@ -17,11 +17,11 @@
 
 package org.photonvision.vision.frame.consumer;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.StringSubscriber;
+import edu.wpi.first.wpilibj.DriverStation.MatchType;
 import java.io.File;
 import java.text.DateFormat;
 import java.text.SimpleDateFormat;
@@ -38,21 +38,18 @@ import org.photonvision.vision.opencv.CVMat;
 public class FileSaveFrameConsumer implements Consumer<CVMat> {
     private final Logger logger = new Logger(FileSaveFrameConsumer.class, LogGroup.General);
 
-    // match type's values from the FMS.
-    private static final String[] matchTypes = {"N/A", "P", "Q", "E", "EV"};
-
     // Formatters to generate unique, timestamped file names
     private static final String FILE_PATH = ConfigManager.getInstance().getImageSavePath().toString();
-    private static final String FILE_EXTENSION = ".jpg";
+    static final String FILE_EXTENSION = ".jpg";
     private static final String NT_SUFFIX = "SaveImgCmd";
 
-    DateFormat df = new SimpleDateFormat("yyyy-MM-dd");
-    DateFormat tf = new SimpleDateFormat("hhmmssSS");
+    static final DateFormat df = new SimpleDateFormat("yyyy-MM-dd");
+    static final DateFormat tf = new SimpleDateFormat("hhmmssSS");
 
     private final NetworkTable rootTable;
     private NetworkTable subTable;
     private final String ntEntryName;
-    private IntegerEntry saveFrameEntry;
+    IntegerEntry saveFrameEntry;
 
     private StringSubscriber ntEventName;
     private IntegerSubscriber ntMatchNum;
@@ -74,13 +71,17 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
 
         NetworkTable fmsTable = NetworkTablesManager.getInstance().getNTInst().getTable("FMSInfo");
         this.ntEventName = fmsTable.getStringTopic("EventName").subscribe("UNKNOWN");
-        this.ntMatchNum = fmsTable.getIntegerTopic("MatchNumber").subscribe(-1);
+        this.ntMatchNum = fmsTable.getIntegerTopic("MatchNumber").subscribe(0);
         this.ntMatchType = fmsTable.getIntegerTopic("MatchType").subscribe(0);
 
         updateCameraNickname(camNickname);
     }
 
     public void accept(CVMat image) {
+        accept(image, new Date());
+    }
+
+    public void accept(CVMat image, Date now) {
         long currentCount = saveFrameEntry.get();
 
         // Await save request
@@ -88,7 +89,7 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
 
         // The requested count is greater than the actual count
         if (savedImagesCount < currentCount) {
-            Date now = new Date();
+            String matchData = getMatchData();
 
             String fileName =
                     cameraNickname
@@ -99,7 +100,7 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
                             + "T"
                             + tf.format(now)
                             + "_"
-                            + getMatchData();
+                            + matchData;
 
             // Check if the Unique Camera directory exists and create it if it doesn't
             String cameraPath = FILE_PATH + File.separator + this.cameraUniqueName;
@@ -110,6 +111,7 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
 
             String saveFilePath = cameraPath + File.separator + fileName + FILE_EXTENSION;
 
+            logger.info("Saving image to: " + saveFilePath);
             if (image == null || image.getMat() == null || image.getMat().empty()) {
                 Imgcodecs.imwrite(saveFilePath, StaticFrames.LOST_MAT);
             } else {
@@ -145,19 +147,19 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
     }
 
     /**
-     * Returns the match Data collected from the NT. eg : Q58 for qualfication match 58. If not in
-     * event, returns N/A-0-EVENTNAME
+     * Returns the match Data collected from the NT. eg : Q58 for qualification match 58. If not in
+     * event, returns None-0-Unknown
      */
     private String getMatchData() {
         var matchType = ntMatchType.getAtomic();
         if (matchType.timestamp == 0) {
             // no NT info yet
-            logger.warn("Did not receive match type, defaulting to 0");
+            logger.warn("Did not receive match type, defaulting to None");
         }
 
         var matchNum = ntMatchNum.getAtomic();
         if (matchNum.timestamp == 0) {
-            logger.warn("Did not receive match num, defaulting to -1");
+            logger.warn("Did not receive match num, defaulting to 0");
         }
 
         var eventName = ntEventName.getAtomic();
@@ -165,9 +167,14 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
             logger.warn("Did not receive event name, defaulting to 'UNKNOWN'");
         }
 
-        String matchTypeStr =
-                matchTypes[MathUtil.clamp((int) matchType.value, 0, matchTypes.length - 1)];
-        return matchTypeStr + "-" + matchNum.value + "-" + eventName.value;
+        MatchType wpiMatchType = MatchType.None; // Default is to be unknown
+        if (matchType.value < 0 || matchType.value >= MatchType.values().length) {
+            logger.error("Invalid match type from FMS: " + matchType.value);
+        } else {
+            wpiMatchType = MatchType.values()[(int) matchType.value];
+        }
+
+        return wpiMatchType.name() + "-" + matchNum.value + "-" + eventName.value;
     }
 
     public void close() {

photon-core/src/main/java/org/photonvision/vision/objects/RknnModel.java

@@ -23,6 +23,7 @@ import java.nio.file.Files;
 import java.nio.file.Paths;
 import java.util.List;
 import org.opencv.core.Size;
+import org.photonvision.common.configuration.NeuralNetworkModelManager;
 import org.photonvision.jni.RknnObjectDetector;
 import org.photonvision.rknn.RknnJNI;
 
@@ -39,6 +40,8 @@ public class RknnModel implements Model {
      *
      * <p>"yolov8" -> "YOLO_V8"
      *
+     * <p>"yolov11" -> "YOLO_V11"
+     *
      * @param modelName The model's filename
      * @return The model version
      */
@@ -48,6 +51,8 @@ public class RknnModel implements Model {
             return RknnJNI.ModelVersion.YOLO_V5;
         } else if (modelName.contains("yolov8")) {
             return RknnJNI.ModelVersion.YOLO_V8;
+        } else if (modelName.contains("yolov11")) {
+            return RknnJNI.ModelVersion.YOLO_V11;
         } else {
             throw new IllegalArgumentException("Unknown model version for model " + modelName);
         }
@@ -63,10 +68,8 @@ public class RknnModel implements Model {
     public RknnModel(File modelFile, String labels) throws IllegalArgumentException, IOException {
         this.modelFile = modelFile;
 
-        String[] parts = modelFile.getName().split("-");
-        if (parts.length != 4) {
-            throw new IllegalArgumentException("Invalid model file name: " + modelFile);
-        }
+        // parseRKNNName throws an IllegalArgumentException if the model name is invalid
+        String[] parts = NeuralNetworkModelManager.parseRKNNName(modelFile.getName());
 
         this.version = getModelVersion(parts[3]);
 

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -68,10 +68,12 @@ public class DriverModePipeline
         if (!emptyIn) {
             totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
 
-            var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
+            if (settings.crosshair) {
+                var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
 
-            // calculate elapsed nanoseconds
-            totalNanos += draw2dCrosshairResult.nanosElapsed;
+                // calculate elapsed nanoseconds
+                totalNanos += draw2dCrosshairResult.nanosElapsed;
+            }
         }
 
         var fpsResult = calculateFPSPipe.run(null);
@@ -92,6 +94,7 @@ public class DriverModePipeline
 
     @Override
     public void release() {
-        // we never actually need to give resources up since pipelinemanager only makes one of us
+        // we never actually need to give resources up since pipelinemanager only makes
+        // one of us
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java

@@ -24,6 +24,7 @@ import org.photonvision.vision.processes.PipelineManager;
 @JsonTypeName("DriverModePipelineSettings")
 public class DriverModePipelineSettings extends CVPipelineSettings {
     public DoubleCouple offsetPoint = new DoubleCouple();
+    public boolean crosshair = true;
 
     public DriverModePipelineSettings() {
         super();

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -444,7 +444,7 @@ public class VisionModule {
             return false;
         }
 
-        visionRunner.runSyncronously(
+        visionRunner.runSynchronously(
                 () -> {
                     settables.setVideoModeInternal(pipelineSettings.cameraVideoModeIndex);
                     settables.setBrightness(pipelineSettings.cameraBrightness);

photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java

@@ -206,7 +206,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
             parentModule.startCalibration(deserialized);
             parentModule.saveAndBroadcastAll();
         } catch (Exception e) {
-            logger.error("Error deserailizing start-calibration request", e);
+            logger.error("Error deserializing start-calibration request", e);
         }
     }
 

photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java

@@ -92,7 +92,7 @@ public class VisionRunner {
         }
     }
 
-    public Future<Void> runSyncronously(Runnable runnable) {
+    public Future<Void> runSynchronously(Runnable runnable) {
         CompletableFuture<Void> future = new CompletableFuture<>();
 
         synchronized (runnableList) {
@@ -109,7 +109,7 @@ public class VisionRunner {
         return future;
     }
 
-    public <T> Future<T> runSyncronously(Callable<T> callable) {
+    public <T> Future<T> runSynchronously(Callable<T> callable) {
         CompletableFuture<T> future = new CompletableFuture<>();
 
         synchronized (runnableList) {

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java

@@ -366,7 +366,7 @@ public class VisionSourceManager {
      * CameraConfiguration}'s matchedCameraInfo. We depend on the underlying {@link VisionSource} to
      * be robust to disconnected sources at boot
      *
-     * <p>Verify that nickname is unique within the set of desesrialized camera configurations, adding
+     * <p>Verify that nickname is unique within the set of deserialized camera configurations, adding
      * random characters if this isn't the case
      */
     protected VisionSource loadVisionSourceFromCamConfig(CameraConfiguration configuration) {

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java

@@ -70,7 +70,7 @@ public abstract class VisionSourceSettables {
     public abstract void setGain(int gain);
 
     // Pretty uncommon so instead of abstract this is just a no-op by default
-    // Overriddenn by cameras with AWB gain support
+    // Overridden by cameras with AWB gain support
     public void setRedGain(int red) {}
 
     public void setBlueGain(int blue) {}

photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java

@@ -108,6 +108,25 @@ public enum TargetModel implements Releasable {
                             -Units.inchesToMeters(9.5) / 2,
                             -Units.inchesToMeters(9.5) / 2)),
             0),
+    k2025Algae(
+            List.of(
+                    new Point3(
+                            -Units.inchesToMeters(16.25) / 2,
+                            -Units.inchesToMeters(16.25) / 2,
+                            -Units.inchesToMeters(16.25) / 2),
+                    new Point3(
+                            -Units.inchesToMeters(16.25) / 2,
+                            Units.inchesToMeters(16.25) / 2,
+                            -Units.inchesToMeters(16.25) / 2),
+                    new Point3(
+                            Units.inchesToMeters(16.25) / 2,
+                            Units.inchesToMeters(16.25) / 2,
+                            -Units.inchesToMeters(16.25) / 2),
+                    new Point3(
+                            Units.inchesToMeters(16.25) / 2,
+                            -Units.inchesToMeters(16.25) / 2,
+                            -Units.inchesToMeters(16.25) / 2)),
+            0),
     // 2023 AprilTag, with 6 inch marker width (inner black square).
     @JsonAlias({"k6in_16h5"})
     kAprilTag6in_16h5(

photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java

@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.frame.consumer;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.MatchType;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.io.File;
+import java.io.IOException;
+import java.util.Date;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junitpioneer.jupiter.cartesian.CartesianTest;
+import org.junitpioneer.jupiter.cartesian.CartesianTest.Enum;
+import org.junitpioneer.jupiter.cartesian.CartesianTest.Values;
+import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
+import org.photonvision.common.util.TestUtils;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.WpilibLoader;
+import org.photonvision.vision.frame.provider.FileFrameProvider;
+
+public class FileSaveFrameConsumerTest {
+    NetworkTableInstance inst = null;
+
+    @BeforeAll
+    public static void init() throws UnsatisfiedLinkError, IOException {
+        if (!WpilibLoader.loadLibraries()) {
+            fail();
+        }
+
+        try {
+            if (!PhotonTargetingJniLoader.load()) fail();
+        } catch (UnsatisfiedLinkError | IOException e) {
+            e.printStackTrace();
+            fail(e);
+        }
+    }
+
+    @BeforeEach
+    public void setup() {
+        assertNull(inst);
+
+        HAL.initialize(500, 0);
+
+        inst = NetworkTablesManager.getInstance().getNTInst();
+        inst.stopClient();
+        inst.stopServer();
+        inst.startLocal();
+        SmartDashboard.setNetworkTableInstance(inst);
+
+        // DriverStation uses the default instance internally
+        assertEquals(NetworkTableInstance.getDefault(), inst);
+    }
+
+    @AfterEach
+    public void teardown() {
+        SimHooks.resumeTiming();
+
+        HAL.shutdown();
+    }
+
+    @CartesianTest
+    public void testNoMatch(
+            @Enum(MatchType.class) MatchType matchType, @Values(ints = {0, 1, 0xffff}) int matchNumber) {
+        String camNickname = "foobar";
+        String cameraUniqueName = "some_unique";
+        String streamPrefix = "input";
+
+        // GIVEN an input consumer
+        FileSaveFrameConsumer consumer =
+                new FileSaveFrameConsumer(camNickname, cameraUniqueName, streamPrefix);
+
+        // AND a frameProvider giving a random test mode image
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in, false),
+                        TestUtils.WPI2019Image.FOV);
+
+        // AND fake FMS data
+        String eventName = "CASJ";
+        DriverStationSim.setMatchType(matchType);
+        DriverStationSim.setMatchNumber(matchNumber);
+        DriverStationSim.setEventName(eventName);
+        DriverStation.refreshData();
+
+        // WHEN we save the image
+        var currentTime = new Date();
+        var counterPublisher = consumer.saveFrameEntry.getTopic().publish();
+        counterPublisher.accept(1);
+        consumer.accept(frameProvider.get().colorImage, currentTime);
+
+        // THEN an image will be created on disk
+        File expectedSnapshot =
+                ConfigManager.getInstance()
+                        .getImageSavePath()
+                        .resolve(cameraUniqueName)
+                        .resolve(
+                                camNickname
+                                        + "_"
+                                        + streamPrefix
+                                        + "_"
+                                        + FileSaveFrameConsumer.df.format(currentTime)
+                                        + "T"
+                                        + FileSaveFrameConsumer.tf.format(currentTime)
+                                        + "_"
+                                        + (matchType.name() + "-" + matchNumber + "-" + eventName)
+                                        + FileSaveFrameConsumer.FILE_EXTENSION)
+                        .toFile();
+        System.out.println("Checking that file exists: " + expectedSnapshot.getAbsolutePath());
+        assertTrue(expectedSnapshot.exists());
+    }
+}

photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java

@@ -287,7 +287,7 @@ public class Calibrate3dPipeTest {
     }
 
     /**
-     * Uses a given camera coefficientss matrix set to "undistort" every image file found in a given
+     * Uses a given camera coefficients matrix set to "undistort" every image file found in a given
      * directory and display them. Provides an easy way to visually debug the results of the
      * calibration routine. Seems to play havoc with CI and takes a chunk of time, so shouldn't
      * usually be left active in tests.

photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java

@@ -139,7 +139,7 @@ public class CalibrationRotationPipeTest {
 
         Point[] originalPoints = {new Point(100, 100), new Point(200, 200), new Point(300, 100)};
 
-        // Distort the origonal points
+        // Distort the original points
         var distortedOriginalPoints =
                 OpenCVHelp.distortPoints(
                         List.of(originalPoints),

photon-core/src/test/java/org/photonvision/vision/pipeline/ObjectDetectionTest.java

@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import java.util.LinkedList;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+import org.photonvision.common.configuration.NeuralNetworkModelManager;
+
+public class ObjectDetectionTest {
+    private static LinkedList<String[]> passNames =
+            new LinkedList<String[]>(
+                    java.util.Arrays.asList(
+                            new String[] {"note-640-640-yolov5s.rknn", "note-640-640-yolov5s-labels.txt"},
+                            new String[] {"object-640-640-yolov8n.rknn", "object-640-640-yolov8n-labels.txt"},
+                            new String[] {
+                                "example_1.2-640-640-yolov5l.rknn", "example_1.2-640-640-yolov5l-labels.txt"
+                            },
+                            new String[] {"demo_3.5-640-640-yolov8m.rknn", "demo_3.5-640-640-yolov8m-labels.txt"},
+                            new String[] {"sample-640-640-yolov5x.rknn", "sample-640-640-yolov5x-labels.txt"},
+                            new String[] {
+                                "test_case-640-640-yolov8s.rknn", "test_case-640-640-yolov8s-labels.txt"
+                            },
+                            new String[] {
+                                "model_ABC-640-640-yolov5n.rknn", "model_ABC-640-640-yolov5n-labels.txt"
+                            },
+                            new String[] {"my_model-640-640-yolov8x.rknn", "my_model-640-640-yolov8x-labels.txt"},
+                            new String[] {"name_1.0-640-640-yolov5n.rknn", "name_1.0-640-640-yolov5n-labels.txt"},
+                            new String[] {
+                                "valid_name-640-640-yolov8s.rknn", "valid_name-640-640-yolov8s-labels.txt"
+                            },
+                            new String[] {
+                                "test.model-640-640-yolov5l.rknn", "test.model-640-640-yolov5l-labels.txt"
+                            },
+                            new String[] {
+                                "case1_test-640-640-yolov8m.rknn", "case1_test-640-640-yolov8m-labels.txt"
+                            },
+                            new String[] {"A123-640-640-yolov5x.rknn", "A123-640-640-yolov5x-labels.txt"},
+                            new String[] {
+                                "z_y_test.model-640-640-yolov8n.rknn", "z_y_test.model-640-640-yolov8n-labels.txt"
+                            }));
+    private static LinkedList<String[]> parsedPassNames =
+            new LinkedList<String[]>(
+                    java.util.Arrays.asList(
+                            new String[] {"note", "640", "640", "yolov5s"},
+                            new String[] {"object", "640", "640", "yolov8n"},
+                            new String[] {"example_1.2", "640", "640", "yolov5l"},
+                            new String[] {"demo_3.5", "640", "640", "yolov8m"},
+                            new String[] {"sample", "640", "640", "yolov5x"},
+                            new String[] {"test_case", "640", "640", "yolov8s"},
+                            new String[] {"model_ABC", "640", "640", "yolov5n"},
+                            new String[] {"my_model", "640", "640", "yolov8x"},
+                            new String[] {"name_1.0", "640", "640", "yolov5n"},
+                            new String[] {"valid_name", "640", "640", "yolov8s"},
+                            new String[] {"test.model", "640", "640", "yolov5l"},
+                            new String[] {"case1_test", "640", "640", "yolov8m"},
+                            new String[] {"A123", "640", "640", "yolov5x"},
+                            new String[] {"z_y_test.model", "640", "640", "yolov8n"}));
+    private static LinkedList<String[]> failNames =
+            new LinkedList<String[]>(
+                    java.util.Arrays.asList(
+                            new String[] {"note-yolov5s.rknn", "note-640-640-yolov5s-labels.txt"},
+                            new String[] {"640-640-yolov8n.rknn", "object-640-640-yolov8n-labels.txt"},
+                            new String[] {"example_1.2.rknn", "example_1.2-640-640-yolov5l-labels.txt"},
+                            new String[] {"demo_3.5-640-yolov8m.rknn", "demo_3.5-640-640-yolov8m-labels.txt"},
+                            new String[] {"sample-640.rknn", "sample-640-640-yolov5x-labels.txt"},
+                            new String[] {"test_case.txt", "test_case-640-640-yolov8s-labels.txt"},
+                            new String[] {"model_ABC.onnx", "model_ABC-640-640-yolov5n-labels.txt"},
+                            new String[] {"my_model", "my_model-640-640-yolov8x-labels.txt"},
+                            new String[] {"name_1.0-yolov5n.rknn", "wrong-labels.txt"},
+                            new String[] {"", "valid_name-640-640-yolov8s-labels.txt"},
+                            new String[] {null, "test.model-640-640-yolov5l-labels.txt"},
+                            new String[] {"case1_test-640-640-yolov8m.rknn", null},
+                            new String[] {"A123-640-640.rknn", "different-labels.txt"},
+                            new String[] {"z_y_test.model", ""}));
+
+    // Test the model name validation for names that ought to pass
+    @ParameterizedTest
+    @MethodSource("verifyPassNameProvider")
+    public void testRKNNVerificationPass(String[] names) {
+        NeuralNetworkModelManager.verifyRKNNNames(names[0], names[1]);
+    }
+
+    // // Test the model name validation for names that ought to fail
+    @ParameterizedTest
+    @MethodSource("verifyFailNameProvider")
+    public void testRNNVerificationFail(String[] names) {
+        assertThrows(
+                IllegalArgumentException.class,
+                () -> NeuralNetworkModelManager.verifyRKNNNames(names[0], names[1]));
+    }
+
+    // Test the model name parsing
+    @ParameterizedTest
+    @MethodSource("parseNameProvider")
+    public void testRKNNNameParsing(String[] expected, String name) {
+        String[] parsed = NeuralNetworkModelManager.parseRKNNName(name);
+        assertArrayEquals(expected, parsed);
+    }
+
+    static Stream<Arguments> verifyPassNameProvider() {
+        return passNames.stream().map(array -> Arguments.of((Object) array));
+    }
+
+    static Stream<Arguments> verifyFailNameProvider() {
+        return failNames.stream().map(array -> Arguments.of((Object) array));
+    }
+
+    static Stream<Arguments> parseNameProvider() {
+        // return a stream of parsed pass names, and the first element of each pass name
+        return passNames.stream()
+                .map(name -> Arguments.of(parsedPassNames.get(passNames.indexOf(name)), name[0]));
+    }
+}

photon-lib/py/photonlibpy/__init__.py

@@ -22,7 +22,15 @@
 # SOFTWARE.
 #
 
-from .estimatedRobotPose import EstimatedRobotPose  # noqa
-from .packet import Packet  # noqa
-from .photonCamera import PhotonCamera  # noqa
-from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy  # noqa
+from .estimatedRobotPose import EstimatedRobotPose
+from .packet import Packet
+from .photonCamera import PhotonCamera
+from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy
+
+__all__ = (
+    "EstimatedRobotPose",
+    "Packet",
+    "PhotonCamera",
+    "PhotonPoseEstimator",
+    "PoseStrategy",
+)

photon-lib/py/photonlibpy/simulation/__init__.py

@@ -3,3 +3,11 @@ from .simCameraProperties import SimCameraProperties
 from .videoSimUtil import VideoSimUtil
 from .visionSystemSim import VisionSystemSim
 from .visionTargetSim import VisionTargetSim
+
+__all__ = (
+    "PhotonCameraSim",
+    "SimCameraProperties",
+    "VideoSimUtil",
+    "VisionSystemSim",
+    "VisionTargetSim",
+)

photon-lib/py/photonlibpy/simulation/photonCameraSim.py

@@ -477,11 +477,11 @@ class PhotonCameraSim:
 
         intrinsics = self.prop.getIntrinsics()
         intrinsicsView = intrinsics.flatten().tolist()
-        self.ts.cameraIntrinsicsPublisher.set(intrinsicsView, receiveTimestamp_us)
+        self.ts.cameraIntrinsicsPublisher.set(list(intrinsicsView), receiveTimestamp_us)
 
         distortion = self.prop.getDistCoeffs()
         distortionView = distortion.flatten().tolist()
-        self.ts.cameraDistortionPublisher.set(distortionView, receiveTimestamp_us)
+        self.ts.cameraDistortionPublisher.set(list(distortionView), receiveTimestamp_us)
 
         self.ts.heartbeatPublisher.set(self.heartbeatCounter, receiveTimestamp_us)
         self.heartbeatCounter += 1

photon-lib/py/photonlibpy/targeting/__init__.py

@@ -1,6 +1,13 @@
-# no one but us chickens
+from .multiTargetPNPResult import MultiTargetPNPResult, PnpResult
+from .photonPipelineResult import PhotonPipelineMetadata, PhotonPipelineResult
+from .photonTrackedTarget import PhotonTrackedTarget
+from .TargetCorner import TargetCorner
 
-from .multiTargetPNPResult import MultiTargetPNPResult, PnpResult  # noqa
-from .photonPipelineResult import PhotonPipelineMetadata, PhotonPipelineResult  # noqa
-from .photonTrackedTarget import PhotonTrackedTarget  # noqa
-from .TargetCorner import TargetCorner  # noqa
+__all__ = (
+    "MultiTargetPNPResult",
+    "PnpResult",
+    "PhotonPipelineMetadata",
+    "PhotonPipelineResult",
+    "PhotonTrackedTarget",
+    "TargetCorner",
+)

photon-lib/py/setup.py

@@ -4,7 +4,9 @@ import subprocess
 from setuptools import find_packages, setup
 
 gitDescribeResult = (
-    subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"])
+    subprocess.check_output(
+        ["git", "describe", "--tags", "--match=v*", "--exclude=*rc*", "--always"]
+    )
     .decode("utf-8")
     .strip()
 )
@@ -18,7 +20,7 @@ m = re.search(
 # which should be PEP440 compliant
 if m:
     versionString = m.group(0)
-    # Hack -- for strings like v2024.1.1, do NOT add matruity/suffix
+    # Hack -- for strings like v2024.1.1, do NOT add maturity/suffix
     if len(m.group(2)) > 0:
         print("using beta group matcher")
         prefix = m.group(1)
@@ -59,11 +61,11 @@ setup(
     version=versionString,
     install_requires=[
         "numpy~=2.1",
-        "wpilib<2026,>=2025.2.1",
-        "robotpy-wpimath<2026,>=2025.2.1",
-        "robotpy-apriltag<2026,>=2025.2.1",
-        "robotpy-cscore<2026,>=2025.2.1",
-        "pyntcore<2026,>=2025.2.1",
+        "wpilib<2026,>=2025.3.2",
+        "robotpy-wpimath<2026,>=2025.3.2",
+        "robotpy-apriltag<2026,>=2025.3.2",
+        "robotpy-cscore<2026,>=2025.3.2",
+        "pyntcore<2026,>=2025.3.2",
         "opencv-python;platform_machine!='roborio'",
     ],
     description=descriptionStr,

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -41,13 +41,17 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.StringSubscriber;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.util.WPILibVersion;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 import java.util.Optional;
 import java.util.stream.Collectors;
+import org.opencv.core.Core;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.common.networktables.PacketSubscriber;
 import org.photonvision.targeting.PhotonPipelineResult;
@@ -57,6 +61,7 @@ import org.photonvision.timesync.TimeSyncSingleton;
 public class PhotonCamera implements AutoCloseable {
     private static int InstanceCount = 0;
     public static final String kTableName = "photonvision";
+    private static final String PHOTON_ALERT_GROUP = "PhotonAlerts";
 
     private final NetworkTable cameraTable;
     PacketSubscriber<PhotonPipelineResult> resultSubscriber;
@@ -66,7 +71,7 @@ public class PhotonCamera implements AutoCloseable {
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
     IntegerSubscriber pipelineIndexState, ledModeState;
-    IntegerSubscriber heartbeatEntry;
+    IntegerSubscriber heartbeatSubscriber;
     DoubleArraySubscriber cameraIntrinsicsSubscriber;
     DoubleArraySubscriber cameraDistortionSubscriber;
     MultiSubscriber topicNameSubscriber;
@@ -104,6 +109,9 @@ public class PhotonCamera implements AutoCloseable {
     double prevTimeSyncWarnTime = 0;
     private static final double WARN_DEBOUNCE_SEC = 5;
 
+    private final Alert disconnectAlert;
+    private final Alert timesyncAlert;
+
     public static void setVersionCheckEnabled(boolean enabled) {
         VERSION_CHECK_ENABLED = enabled;
     }
@@ -118,6 +126,10 @@ public class PhotonCamera implements AutoCloseable {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
+        disconnectAlert =
+                new Alert(
+                        PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning);
+        timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", AlertType.kWarning);
         rootPhotonTable = instance.getTable(kTableName);
         this.cameraTable = rootPhotonTable.getSubTable(cameraName);
         path = cameraTable.getPath();
@@ -137,7 +149,7 @@ public class PhotonCamera implements AutoCloseable {
         outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
         pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish();
         pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0);
-        heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
+        heartbeatSubscriber = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
         cameraIntrinsicsSubscriber =
                 cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null);
         cameraDistortionSubscriber =
@@ -157,6 +169,76 @@ public class PhotonCamera implements AutoCloseable {
 
         // HACK - start a TimeSyncServer, if we haven't yet.
         TimeSyncSingleton.load();
+
+        // HACK - check if things are compatible
+        verifyDependencies();
+    }
+
+    public static void verifyDependencies() {
+        if (!WPILibVersion.Version.equals(PhotonVersion.wpilibTargetVersion)) {
+            String bfw =
+                    "\n\n\n\n\n"
+                            + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
+                            + ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+                            + ">>>                                          \n"
+                            + ">>> You are running an incompatible version  \n"
+                            + ">>> of PhotonVision !                        \n"
+                            + ">>>                                          \n"
+                            + ">>> PhotonLib "
+                            + PhotonVersion.versionString
+                            + " is built for WPILib "
+                            + PhotonVersion.wpilibTargetVersion
+                            + "\n"
+                            + ">>> but you are using WPILib "
+                            + WPILibVersion.Version
+                            + ">>>                                          \n"
+                            + ">>> This is neither tested nor supported.    \n"
+                            + ">>> You MUST update PhotonVision,            \n"
+                            + ">>> PhotonLib, or both.                      \n"
+                            + ">>> Verify the output of `./gradlew dependencies` \n"
+                            + ">>>                                          \n"
+                            + ">>> Your code will now crash.                \n"
+                            + ">>> We hope your day gets better.            \n"
+                            + ">>>                                          \n"
+                            + ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+                            + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
+
+            DriverStation.reportWarning(bfw, false);
+            DriverStation.reportError(bfw, false);
+            throw new UnsupportedOperationException(bfw);
+        }
+        if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) {
+            String bfw =
+                    "\n\n\n\n\n"
+                            + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
+                            + ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+                            + ">>>                                          \n"
+                            + ">>> You are running an incompatible version  \n"
+                            + ">>> of PhotonVision !                        \n"
+                            + ">>>                                          \n"
+                            + ">>> PhotonLib "
+                            + PhotonVersion.versionString
+                            + " is built for OpenCV "
+                            + PhotonVersion.opencvTargetVersion
+                            + "\n"
+                            + ">>> but you are using OpenCV "
+                            + Core.VERSION
+                            + ">>>                                          \n"
+                            + ">>> This is neither tested nor supported.    \n"
+                            + ">>> You MUST update PhotonVision,            \n"
+                            + ">>> PhotonLib, or both.                      \n"
+                            + ">>> Verify the output of `./gradlew dependencies` \n"
+                            + ">>>                                          \n"
+                            + ">>> Your code will now crash.                \n"
+                            + ">>> We hope your day gets better.            \n"
+                            + ">>>                                          \n"
+                            + ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+                            + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
+
+            DriverStation.reportWarning(bfw, false);
+            DriverStation.reportError(bfw, false);
+            throw new UnsupportedOperationException(bfw);
+        }
     }
 
     /**
@@ -177,6 +259,7 @@ public class PhotonCamera implements AutoCloseable {
      */
     public List<PhotonPipelineResult> getAllUnreadResults() {
         verifyVersion();
+        updateDisconnectAlert();
 
         List<PhotonPipelineResult> ret = new ArrayList<>();
 
@@ -202,6 +285,7 @@ public class PhotonCamera implements AutoCloseable {
     @Deprecated(since = "2024", forRemoval = true)
     public PhotonPipelineResult getLatestResult() {
         verifyVersion();
+        updateDisconnectAlert();
 
         // Grab the latest result. We don't care about the timestamp from NT - the metadata header has
         // this, latency compensated by the Time Sync Client
@@ -216,22 +300,34 @@ public class PhotonCamera implements AutoCloseable {
         return result;
     }
 
+    private void updateDisconnectAlert() {
+        disconnectAlert.set(!isConnected());
+    }
+
     private void checkTimeSyncOrWarn(PhotonPipelineResult result) {
         if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
+            String warningText =
+                    "PhotonVision coprocessor at path "
+                            + path
+                            + " is not connected to the TimeSyncServer? It's been "
+                            + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
+                            + "s since the coprocessor last heard a pong.";
+
+            timesyncAlert.setText(warningText);
+            timesyncAlert.set(true);
+
             if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
                 prevTimeSyncWarnTime = Timer.getFPGATimestamp();
 
                 DriverStation.reportWarning(
-                        "PhotonVision coprocessor at path "
-                                + path
-                                + " is not connected to the TimeSyncServer? It's been "
-                                + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
-                                + "s since the coprocessor last heard a pong.\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
+                        warningText
+                                + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
                         false);
             }
         } else {
             // Got a valid packet, reset the last time
             prevTimeSyncWarnTime = 0;
+            timesyncAlert.set(false);
         }
     }
 
@@ -332,9 +428,14 @@ public class PhotonCamera implements AutoCloseable {
      * @return True if the camera is actively sending frame data, false otherwise.
      */
     public boolean isConnected() {
-        var curHeartbeat = heartbeatEntry.get();
+        var curHeartbeat = heartbeatSubscriber.get();
         var now = Timer.getFPGATimestamp();
 
+        if (curHeartbeat < 0) {
+            // we have never heard from the camera
+            return false;
+        }
+
         if (curHeartbeat != prevHeartbeatValue) {
             // New heartbeat value from the coprocessor
             prevHeartbeatChangeTime = now;
@@ -383,7 +484,7 @@ public class PhotonCamera implements AutoCloseable {
 
         // Heartbeat entry is assumed to always be present. If it's not present, we
         // assume that a camera with that name was never connected in the first place.
-        if (!heartbeatEntry.exists()) {
+        if (!heartbeatSubscriber.exists()) {
             var cameraNames = getTablesThatLookLikePhotonCameras();
             if (cameraNames.isEmpty()) {
                 DriverStation.reportError(

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -25,15 +25,19 @@
 package org.photonvision;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.cscore.OpenCvLoader;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N8;
@@ -83,9 +87,45 @@ public class PhotonPoseEstimator {
          * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can
          * take a lot of time.
          */
-        MULTI_TAG_PNP_ON_RIO
+        MULTI_TAG_PNP_ON_RIO,
+
+        /**
+         * Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order
+         * to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading
+         * data is up-to-date.
+         *
+         * <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
+         *
+         * <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+         */
+        PNP_DISTANCE_TRIG_SOLVE,
+
+        /**
+         * Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase
+         * flat on the floor. This computation takes place on the RoboRIO, and typically takes not more
+         * than 2ms. See {@link PhotonPoseEstimator.ConstrainedSolvepnpParams} and {@link
+         * org.photonvision.jni.ConstrainedSolvepnpJni} for details and tuning handles this strategy
+         * exposes. This strategy needs addHeadingData called every frame so heading data is up-to-date.
+         * If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to
+         * the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the
+         * seed.
+         */
+        CONSTRAINED_SOLVEPNP
     }
 
+    /**
+     * Tuning handles we have over the CONSTRAINED_SOLVEPNP {@link PhotonPoseEstimator.PoseStrategy}.
+     * Internally, the cost function is a sum-squared of pixel reprojection error + (optionally)
+     * heading error * heading scale factor.
+     *
+     * @param headingFree If true, heading is completely free to vary. If false, heading excursions
+     *     from the provided heading measurement will be penalized
+     * @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
+     *     heading estimate against the tag corner reprojection error const.
+     */
+    public static final record ConstrainedSolvepnpParams(
+            boolean headingFree, double headingScaleFactor) {}
+
     private AprilTagFieldLayout fieldTags;
     private TargetModel tagModel = TargetModel.kAprilTag36h11;
     private PoseStrategy primaryStrategy;
@@ -97,6 +137,9 @@ public class PhotonPoseEstimator {
     protected double poseCacheTimestampSeconds = -1;
     private final Set<Integer> reportedErrors = new HashSet<>();
 
+    private final TimeInterpolatableBuffer<Rotation2d> headingBuffer =
+            TimeInterpolatableBuffer.createBuffer(1.0);
+
     /**
      * Create a new PhotonPoseEstimator.
      *
@@ -117,6 +160,11 @@ public class PhotonPoseEstimator {
         this.primaryStrategy = strategy;
         this.robotToCamera = robotToCamera;
 
+        if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO
+                || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) {
+            OpenCvLoader.forceStaticLoad();
+        }
+
         HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
         InstanceCount++;
     }
@@ -189,6 +237,11 @@ public class PhotonPoseEstimator {
      */
     public void setPrimaryStrategy(PoseStrategy strategy) {
         checkUpdate(this.primaryStrategy, strategy);
+
+        if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO
+                || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) {
+            OpenCvLoader.forceStaticLoad();
+        }
         this.primaryStrategy = strategy;
     }
 
@@ -259,6 +312,30 @@ public class PhotonPoseEstimator {
         setLastPose(new Pose3d(lastPose));
     }
 
+    /**
+     * Add robot heading data to buffer. Must be called periodically for the
+     * <b>PNP_DISTANCE_TRIG_SOLVE</b> strategy.
+     *
+     * @param timestampSeconds timestamp of the robot heading data.
+     * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field
+     *     coordinates.
+     */
+    public void addHeadingData(double timestampSeconds, Rotation3d heading) {
+        addHeadingData(timestampSeconds, heading.toRotation2d());
+    }
+
+    /**
+     * Add robot heading data to buffer. Must be called periodically for the
+     * <b>PNP_DISTANCE_TRIG_SOLVE</b> strategy.
+     *
+     * @param timestampSeconds timestamp of the robot heading data.
+     * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field
+     *     coordinates.
+     */
+    public void addHeadingData(double timestampSeconds, Rotation2d heading) {
+        headingBuffer.addSample(timestampSeconds, heading);
+    }
+
     /**
      * @return The current transform from the center of the robot to the camera mount position
      */
@@ -286,8 +363,8 @@ public class PhotonPoseEstimator {
      *   <li>No targets were found in the pipeline results.
      * </ul>
      *
-     * Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
-     * provided
+     * Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not
+     * provided in this overload.
      *
      * @param cameraResult The latest pipeline result from the camera
      * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
@@ -304,6 +381,8 @@ public class PhotonPoseEstimator {
      *   <li>The timestamp of the provided pipeline result is the same as in the previous call to
      *       {@code update()}.
      *   <li>No targets were found in the pipeline results.
+     *   <li>The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the
+     *       other function overload).
      * </ul>
      *
      * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
@@ -317,6 +396,32 @@ public class PhotonPoseEstimator {
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
             Optional<Matrix<N8, N1>> distCoeffs) {
+        return update(cameraResult, cameraMatrix, distCoeffs, Optional.empty());
+    }
+
+    /**
+     * Updates the estimated position of the robot. Returns empty if:
+     *
+     * <ul>
+     *   <li>The timestamp of the provided pipeline result is the same as in the previous call to
+     *       {@code update()}.
+     *   <li>No targets were found in the pipeline results.
+     *   <li>The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty.
+     * </ul>
+     *
+     * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
+     *     otherwise
+     * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
+     *     otherwise
+     * @param constrainedPnpParams Constrained SolvePNP params, if needed.
+     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    public Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult,
+            Optional<Matrix<N3, N3>> cameraMatrix,
+            Optional<Matrix<N8, N1>> distCoeffs,
+            Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
         // Time in the past -- give up, since the following if expects times > 0
         if (cameraResult.getTimestampSeconds() < 0) {
             return Optional.empty();
@@ -338,13 +443,29 @@ public class PhotonPoseEstimator {
             return Optional.empty();
         }
 
-        return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy);
+        return update(
+                cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, this.primaryStrategy);
+    }
+
+    /**
+     * Internal convenience method for using a fallback strategy for update(). This should only be
+     * called after timestamp checks have been done by another update() overload.
+     *
+     * @param cameraResult The latest pipeline result from the camera
+     * @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP.
+     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    private Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult, PoseStrategy strategy) {
+        return update(cameraResult, Optional.empty(), Optional.empty(), Optional.empty(), strategy);
     }
 
     private Optional<EstimatedRobotPose> update(
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
             Optional<Matrix<N8, N1>> distCoeffs,
+            Optional<ConstrainedSolvepnpParams> constrainedPnpParams,
             PoseStrategy strategy) {
         Optional<EstimatedRobotPose> estimatedPose =
                 switch (strategy) {
@@ -357,22 +478,12 @@ public class PhotonPoseEstimator {
                         yield closestToReferencePoseStrategy(cameraResult, referencePose);
                     }
                     case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
-                    case MULTI_TAG_PNP_ON_RIO -> {
-                        if (cameraMatrix.isEmpty()) {
-                            DriverStation.reportWarning(
-                                    "Camera matrix is empty for multi-tag-on-rio",
-                                    Thread.currentThread().getStackTrace());
-                            yield Optional.empty();
-                        } else if (distCoeffs.isEmpty()) {
-                            DriverStation.reportWarning(
-                                    "Camera matrix is empty for multi-tag-on-rio",
-                                    Thread.currentThread().getStackTrace());
-                            yield Optional.empty();
-                        } else {
-                            yield multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
-                        }
-                    }
+                    case MULTI_TAG_PNP_ON_RIO ->
+                            multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
                     case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
+                    case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult);
+                    case CONSTRAINED_SOLVEPNP ->
+                            constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams);
                 };
 
         if (estimatedPose.isPresent()) {
@@ -382,34 +493,164 @@ public class PhotonPoseEstimator {
         return estimatedPose;
     }
 
-    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
-        if (result.getMultiTagResult().isPresent()) {
-            var best_tf = result.getMultiTagResult().get().estimatedPose.best;
-            var best =
-                    new Pose3d()
-                            .plus(best_tf) // field-to-camera
-                            .relativeTo(fieldTags.getOrigin())
-                            .plus(robotToCamera.inverse()); // field-to-robot
-            return Optional.of(
-                    new EstimatedRobotPose(
-                            best,
-                            result.getTimestampSeconds(),
-                            result.getTargets(),
-                            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
-        } else {
-            // We can never fall back on another multitag strategy
-            return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
+    private Optional<EstimatedRobotPose> pnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
+        PhotonTrackedTarget bestTarget = result.getBestTarget();
+
+        if (bestTarget == null) return Optional.empty();
+
+        var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds());
+        if (headingSampleOpt.isEmpty()) {
+            return Optional.empty();
         }
+        Rotation2d headingSample = headingSampleOpt.get();
+
+        Translation2d camToTagTranslation =
+                new Translation3d(
+                                bestTarget.getBestCameraToTarget().getTranslation().getNorm(),
+                                new Rotation3d(
+                                        0,
+                                        -Math.toRadians(bestTarget.getPitch()),
+                                        -Math.toRadians(bestTarget.getYaw())))
+                        .rotateBy(robotToCamera.getRotation())
+                        .toTranslation2d()
+                        .rotateBy(headingSample);
+
+        var tagPoseOpt = fieldTags.getTagPose(bestTarget.getFiducialId());
+        if (tagPoseOpt.isEmpty()) {
+            return Optional.empty();
+        }
+        var tagPose2d = tagPoseOpt.get().toPose2d();
+
+        Translation2d fieldToCameraTranslation =
+                tagPose2d.getTranslation().plus(camToTagTranslation.unaryMinus());
+
+        Translation2d camToRobotTranslation =
+                robotToCamera.getTranslation().toTranslation2d().unaryMinus().rotateBy(headingSample);
+
+        Pose2d robotPose =
+                new Pose2d(fieldToCameraTranslation.plus(camToRobotTranslation), headingSample);
+
+        return Optional.of(
+                new EstimatedRobotPose(
+                        new Pose3d(robotPose),
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.PNP_DISTANCE_TRIG_SOLVE));
+    }
+
+    private Optional<EstimatedRobotPose> constrainedPnpStrategy(
+            PhotonPipelineResult result,
+            Optional<Matrix<N3, N3>> cameraMatrixOpt,
+            Optional<Matrix<N8, N1>> distCoeffsOpt,
+            Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
+        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
+        // cannot run multitagPNP, use fallback strategy
+        if (!hasCalibData) {
+            return update(
+                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
+        }
+
+        if (constrainedPnpParams.isEmpty()) {
+            return Optional.empty();
+        }
+
+        // Need heading if heading fixed
+        if (!constrainedPnpParams.get().headingFree
+                && headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) {
+            return update(
+                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
+        }
+
+        Pose3d fieldToRobotSeed;
+
+        // Attempt to use multi-tag to get a pose estimate seed
+        if (result.getMultiTagResult().isPresent()) {
+            fieldToRobotSeed =
+                    Pose3d.kZero.plus(
+                            result.getMultiTagResult().get().estimatedPose.best.plus(robotToCamera.inverse()));
+        } else {
+            // HACK - use fallback strategy to gimme a seed pose
+            // TODO - make sure nested update doesn't break state
+            var nestedUpdate =
+                    update(
+                            result,
+                            cameraMatrixOpt,
+                            distCoeffsOpt,
+                            Optional.empty(),
+                            this.multiTagFallbackStrategy);
+            if (nestedUpdate.isEmpty()) {
+                // best i can do is bail
+                return Optional.empty();
+            }
+            fieldToRobotSeed = nestedUpdate.get().estimatedPose;
+        }
+
+        if (!constrainedPnpParams.get().headingFree) {
+            // If heading fixed, force rotation component
+            fieldToRobotSeed =
+                    new Pose3d(
+                            fieldToRobotSeed.getTranslation(),
+                            new Rotation3d(headingBuffer.getSample(result.getTimestampSeconds()).get()));
+        }
+
+        var pnpResult =
+                VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
+                        cameraMatrixOpt.get(),
+                        distCoeffsOpt.get(),
+                        result.getTargets(),
+                        robotToCamera,
+                        fieldToRobotSeed,
+                        fieldTags,
+                        tagModel,
+                        constrainedPnpParams.get().headingFree,
+                        headingBuffer.getSample(result.getTimestampSeconds()).get(),
+                        constrainedPnpParams.get().headingScaleFactor);
+        // try fallback strategy if solvePNP fails for some reason
+        if (!pnpResult.isPresent())
+            return update(
+                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
+        var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot
+
+        return Optional.of(
+                new EstimatedRobotPose(
+                        best,
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP));
+    }
+
+    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
+        if (result.getMultiTagResult().isEmpty()) {
+            return update(result, this.multiTagFallbackStrategy);
+        }
+
+        var best_tf = result.getMultiTagResult().get().estimatedPose.best;
+        var best =
+                Pose3d.kZero
+                        .plus(best_tf) // field-to-camera
+                        .relativeTo(fieldTags.getOrigin())
+                        .plus(robotToCamera.inverse()); // field-to-robot
+        return Optional.of(
+                new EstimatedRobotPose(
+                        best,
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
     }
 
     private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
             PhotonPipelineResult result,
             Optional<Matrix<N3, N3>> cameraMatrixOpt,
             Optional<Matrix<N8, N1>> distCoeffsOpt) {
-        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
-        // cannot run multitagPNP, use fallback strategy
-        if (!hasCalibData || result.getTargets().size() < 2) {
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+        if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
+            DriverStation.reportWarning(
+                    "No camera calibration data provided for multi-tag-on-rio",
+                    Thread.currentThread().getStackTrace());
+            return update(result, this.multiTagFallbackStrategy);
+        }
+
+        if (result.getTargets().size() < 2) {
+            return update(result, this.multiTagFallbackStrategy);
         }
 
         var pnpResult =
@@ -417,9 +658,11 @@ public class PhotonPoseEstimator {
                         cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
         // try fallback strategy if solvePNP fails for some reason
         if (!pnpResult.isPresent())
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+            return update(
+                    result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
+
         var best =
-                new Pose3d()
+                Pose3d.kZero
                         .plus(pnpResult.get().best) // field-to-camera
                         .plus(robotToCamera.inverse()); // field-to-robot
 

photon-lib/src/main/java/org/photonvision/PhotonUtils.java

@@ -172,9 +172,9 @@ public final class PhotonUtils {
      * Estimates the pose of the robot in the field coordinate system, given the pose of the fiducial
      * tag, the robot relative to the camera, and the target relative to the camera.
      *
-     * @param fieldRelativeTagPose Pose3D the field relative pose of the target
      * @param cameraToRobot Transform3D of the robot relative to the camera. Origin of the robot is
      *     defined as the center.
+     * @param fieldRelativeTagPose Pose3D the field relative pose of the target
      * @param cameraToTarget Transform3D of the target relative to the camera, returned by
      *     PhotonVision
      * @return Transform3d Robot position relative to the field

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -28,6 +28,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.OpenCvLoader;
 import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Pair;
@@ -94,7 +95,7 @@ public class PhotonCameraSim implements AutoCloseable {
     private boolean videoSimProcEnabled = true;
 
     static {
-        OpenCVHelp.forceLoadOpenCV();
+        OpenCvLoader.forceStaticLoad();
     }
 
     @Override

photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java

@@ -160,11 +160,12 @@ public class SimCameraProperties {
         if (!success) throw new IOException("Requested resolution not found in calibration");
     }
 
-    public void setRandomSeed(long seed) {
+    public SimCameraProperties setRandomSeed(long seed) {
         rand.setSeed(seed);
+        return this;
     }
 
-    public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
+    public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
         if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) {
             fovDiag = Rotation2d.fromDegrees(MathUtil.clamp(fovDiag.getDegrees(), 1, 179));
             DriverStation.reportError(
@@ -189,9 +190,11 @@ public class SimCameraProperties {
         // create camera intrinsics matrix
         var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1);
         setCalibration(resWidth, resHeight, camIntrinsics, distCoeff);
+
+        return this;
     }
 
-    public void setCalibration(
+    public SimCameraProperties setCalibration(
             int resWidth, int resHeight, Matrix<N3, N3> camIntrinsics, Matrix<N8, N1> distCoeffs) {
         this.resWidth = resWidth;
         this.resHeight = resHeight;
@@ -222,43 +225,54 @@ public class SimCameraProperties {
             viewplanes.add(
                     new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ()));
         }
+
+        return this;
     }
 
-    public void setCalibError(double avgErrorPx, double errorStdDevPx) {
+    public SimCameraProperties setCalibError(double avgErrorPx, double errorStdDevPx) {
         this.avgErrorPx = avgErrorPx;
         this.errorStdDevPx = errorStdDevPx;
+        return this;
     }
 
     /**
      * @param fps The average frames per second the camera should process at. <b>Exposure time limits
      *     FPS if set!</b>
      */
-    public void setFPS(double fps) {
+    public SimCameraProperties setFPS(double fps) {
         frameSpeedMs = Math.max(1000.0 / fps, exposureTimeMs);
+
+        return this;
     }
 
     /**
      * @param exposureTimeMs The amount of time the "shutter" is open for one frame. Affects motion
      *     blur. <b>Frame speed(from FPS) is limited to this!</b>
      */
-    public void setExposureTimeMs(double exposureTimeMs) {
+    public SimCameraProperties setExposureTimeMs(double exposureTimeMs) {
         this.exposureTimeMs = exposureTimeMs;
         frameSpeedMs = Math.max(frameSpeedMs, exposureTimeMs);
+
+        return this;
     }
 
     /**
      * @param avgLatencyMs The average latency (from image capture to data published) in milliseconds
      *     a frame should have
      */
-    public void setAvgLatencyMs(double avgLatencyMs) {
+    public SimCameraProperties setAvgLatencyMs(double avgLatencyMs) {
         this.avgLatencyMs = avgLatencyMs;
+
+        return this;
     }
 
     /**
      * @param latencyStdDevMs The standard deviation in milliseconds of the latency
      */
-    public void setLatencyStdDevMs(double latencyStdDevMs) {
+    public SimCameraProperties setLatencyStdDevMs(double latencyStdDevMs) {
         this.latencyStdDevMs = latencyStdDevMs;
+
+        return this;
     }
 
     public int getResWidth() {

photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java

@@ -26,6 +26,7 @@ package org.photonvision.simulation;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.OpenCvLoader;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
@@ -62,7 +63,7 @@ public class VideoSimUtil {
     private static double fieldWidth = 8.0137;
 
     static {
-        OpenCVHelp.forceLoadOpenCV();
+        OpenCvLoader.forceStaticLoad();
 
         // create Mats of 10x10 apriltag images
         for (int i = 0; i < VideoSimUtil.kNumTags36h11; i++) {

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -27,10 +27,12 @@
 #include <hal/FRCUsageReporting.h>
 #include <net/TimeSyncServer.h>
 
+#include <stdexcept>
 #include <string>
 #include <string_view>
 #include <vector>
 
+#include <WPILibVersion.h>
 #include <frc/Errors.h>
 #include <frc/RobotController.h>
 #include <frc/Timer.h>
@@ -39,26 +41,84 @@
 #include <wpi/json.h>
 
 #include "PhotonVersion.h"
+#include "opencv2/core/utility.hpp"
 #include "photon/dataflow/structures/Packet.h"
 
-inline constexpr std::string_view bfw =
-    "\n\n\n\n"
-    ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
-    ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
-    ">>>                                          \n"
-    ">>> You are running an incompatible version  \n"
-    ">>> of PhotonVision on your coprocessor!     \n"
-    ">>>                                          \n"
-    ">>> This is neither tested nor supported.    \n"
-    ">>> You MUST update PhotonVision,            \n"
-    ">>> PhotonLib, or both.                      \n"
-    ">>>                                          \n"
-    ">>> Your code will now crash.                \n"
-    ">>> We hope your day gets better.            \n"
-    ">>>                                          \n"
-    ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
-    ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
-    "\n\n";
+static constexpr units::second_t WARN_DEBOUNCE_SEC = 5_s;
+static constexpr units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
+
+inline void verifyDependencies() {
+  if (!(std::string_view{GetWPILibVersion()} ==
+        std::string_view{photon::PhotonVersion::wpilibTargetVersion})) {
+    std::string bfw =
+        "\n\n\n\n\n"
+        ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
+        ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+        ">>>                                          \n"
+        ">>> You are running an incompatible version  \n"
+        ">>> of PhotonVision !                        \n"
+        ">>>                                          \n"
+        ">>> PhotonLib ";
+    bfw += photon::PhotonVersion::versionString;
+    bfw += " is built for WPILib ";
+    bfw += photon::PhotonVersion::wpilibTargetVersion;
+    bfw +=
+        "\n"
+        ">>> but you are using WPILib ";
+    bfw += GetWPILibVersion();
+    bfw +=
+        "\n>>>                                          \n"
+        ">>> This is neither tested nor supported.    \n"
+        ">>> You MUST update PhotonVision,            \n"
+        ">>> PhotonLib, or both.                      \n"
+        ">>> Verify the output of `./gradlew dependencies` \n"
+        ">>>                                          \n"
+        ">>> Your code will now crash.                \n"
+        ">>> We hope your day gets better.            \n"
+        ">>>                                          \n"
+        ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+        ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
+
+    FRC_ReportWarning(bfw);
+    FRC_ReportError(frc::err::Error, bfw);
+    throw new std::runtime_error(std::string{bfw});
+  }
+  if (!(std::string_view{cv::getVersionString()} ==
+        std::string_view{photon::PhotonVersion::opencvTargetVersion})) {
+    std::string bfw =
+        "\n\n\n\n\n"
+        ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
+        ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+        ">>>                                          \n"
+        ">>> You are running an incompatible version  \n"
+        ">>> of PhotonVision !                        \n"
+        ">>>                                          \n"
+        ">>> PhotonLib ";
+    bfw += photon::PhotonVersion::versionString;
+    bfw += " is built for OpenCV ";
+    bfw += photon::PhotonVersion::opencvTargetVersion;
+    bfw +=
+        "\n"
+        ">>> but you are using OpenCV ";
+    bfw += cv::getVersionString();
+    bfw +=
+        "\n>>>                                          \n"
+        ">>> This is neither tested nor supported.    \n"
+        ">>> You MUST update PhotonVision,            \n"
+        ">>> PhotonLib, or both.                      \n"
+        ">>> Verify the output of `./gradlew dependencies` \n"
+        ">>>                                          \n"
+        ">>> Your code will now crash.                \n"
+        ">>> We hope your day gets better.            \n"
+        ">>>                                          \n"
+        ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+        ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
+
+    FRC_ReportWarning(bfw);
+    FRC_ReportError(frc::err::Error, bfw);
+    throw new std::runtime_error(std::string{bfw});
+  }
+}
 
 // bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want
 // to avoid calling this from static initialization
@@ -80,6 +140,7 @@ namespace photon {
 
 constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
 static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
+static const std::string PHOTON_ALERT_GROUP{"PhotonAlerts"};
 bool PhotonCamera::VERSION_CHECK_ENABLED = true;
 
 void PhotonCamera::SetVersionCheckEnabled(bool enabled) {
@@ -122,9 +183,17 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
           rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
       driverModePublisher(
           rootTable->GetBooleanTopic("driverModeRequest").Publish()),
+      heartbeatSubscriber(
+          rootTable->GetIntegerTopic("heartbeat").Subscribe(-1)),
       topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
       path(rootTable->GetPath()),
-      cameraName(cameraName) {
+      cameraName(cameraName),
+      disconnectAlert(PHOTON_ALERT_GROUP,
+                      std::string{"PhotonCamera '"} + std::string{cameraName} +
+                          "' is disconnected.",
+                      frc::Alert::AlertType::kWarning),
+      timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) {
+  verifyDependencies();
   HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount);
   InstanceCount++;
 
@@ -159,6 +228,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   // Create the new result;
   PhotonPipelineResult result = packet.Unpack<PhotonPipelineResult>();
 
+  CheckTimeSyncOrWarn(result);
+
   result.SetReceiveTimestamp(now);
 
   return result;
@@ -171,6 +242,7 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
 
   // Prints warning if not connected
   VerifyVersion();
+  UpdateDisconnectAlert();
 
   const auto changes = rawBytesEntry.ReadQueue();
 
@@ -189,6 +261,8 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
     photon::Packet packet{value.value};
     auto result = packet.Unpack<PhotonPipelineResult>();
 
+    CheckTimeSyncOrWarn(result);
+
     // TODO: NT4 timestamps are still not to be trusted. But it's the best we
     // can do until we can make time sync more reliable.
     result.SetReceiveTimestamp(units::microsecond_t(value.time) -
@@ -200,6 +274,37 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
   return ret;
 }
 
+void PhotonCamera::UpdateDisconnectAlert() {
+  disconnectAlert.Set(!IsConnected());
+}
+
+void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) {
+  if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
+    std::string warningText =
+        "PhotonVision coprocessor at path " + path +
+        " is not connected to the TimeSyncServer? It's been " +
+        std::to_string(result.metadata.timeSinceLastPong / 1e6) +
+        "s since the coprocessor last heard a pong.";
+
+    timesyncAlert.SetText(warningText);
+    timesyncAlert.Set(true);
+
+    if (frc::Timer::GetFPGATimestamp() <
+        (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
+      prevTimeSyncWarnTime = frc::Timer::GetFPGATimestamp();
+
+      FRC_ReportWarning(
+          warningText +
+          "\n\nCheck /photonvision/.timesync/{{COPROCESSOR_HOSTNAME}} for more "
+          "information.");
+    }
+  } else {
+    // Got a valid packet, reset the last time
+    prevTimeSyncWarnTime = 0_s;
+    timesyncAlert.Set(false);
+  }
+}
+
 void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModePublisher.Set(driverMode);
 }
@@ -232,6 +337,24 @@ const std::string_view PhotonCamera::GetCameraName() const {
   return cameraName;
 }
 
+bool PhotonCamera::IsConnected() {
+  auto currentHeartbeat = heartbeatSubscriber.Get();
+  auto now = frc::Timer::GetFPGATimestamp();
+
+  if (currentHeartbeat < 0) {
+    // we have never heard from the camera
+    return false;
+  }
+
+  if (currentHeartbeat != prevHeartbeatValue) {
+    // New heartbeat value from the coprocessor
+    prevHeartbeatChangeTime = now;
+    prevHeartbeatValue = currentHeartbeat;
+  }
+
+  return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
+}
+
 std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
   auto camCoeffs = cameraIntrinsicsSubscriber.Get();
   if (camCoeffs.size() == 9) {
@@ -310,7 +433,25 @@ void PhotonCamera::VerifyVersion() {
     std::string remote_uuid{remote_uuid_json};
 
     if (local_uuid != remote_uuid) {
-      FRC_ReportError(frc::warn::Warning, bfw);
+      constexpr std::string_view bfw =
+          "\n\n\n\n"
+          ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
+          ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+          ">>>                                          \n"
+          ">>> You are running an incompatible version  \n"
+          ">>> of PhotonVision on your coprocessor!     \n"
+          ">>>                                          \n"
+          ">>> This is neither tested nor supported.    \n"
+          ">>> You MUST update PhotonVision,            \n"
+          ">>> PhotonLib, or both.                      \n"
+          ">>>                                          \n"
+          ">>> Your code will now crash.                \n"
+          ">>> We hope your day gets better.            \n"
+          ">>>                                          \n"
+          ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
+          ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
+          "\n\n";
+      FRC_ReportWarning(bfw);
       std::string error_str = fmt::format(
           "Photonlib version {} (message definition version {}) does not match "
           "coprocessor version {} (message definition version {})!",

photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp

@@ -350,18 +350,17 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
     PhotonPipelineResult result) {
-  if (result.MultiTagResult()) {
-    const auto field2camera = result.MultiTagResult()->estimatedPose.best;
-
-    const auto fieldToRobot =
-        frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
-    return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
-                                      result.GetTargets(),
-                                      MULTI_TAG_PNP_ON_COPROCESSOR);
+  if (!result.MultiTagResult()) {
+    return Update(result, this->multiTagFallbackStrategy);
   }
 
-  return Update(result, std::nullopt, std::nullopt,
-                this->multiTagFallbackStrategy);
+  const auto field2camera = result.MultiTagResult()->estimatedPose.best;
+
+  const auto fieldToRobot =
+      frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
+  return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
+                                    result.GetTargets(),
+                                    MULTI_TAG_PNP_ON_COPROCESSOR);
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
@@ -370,19 +369,17 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
     std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
   using namespace frc;
 
-  // Need at least 2 targets
-  if (!result.HasTargets() || result.GetTargets().size() < 2) {
-    return Update(result, std::nullopt, std::nullopt,
-                  this->multiTagFallbackStrategy);
-  }
-
   if (!camMat || !distCoeffs) {
     FRC_ReportError(frc::warn::Warning,
                     "No camera calibration data provided to "
                     "PhotonPoseEstimator::MultiTagOnRioStrategy!",
                     "");
-    return Update(result, std::nullopt, std::nullopt,
-                  this->multiTagFallbackStrategy);
+    return Update(result, this->multiTagFallbackStrategy);
+  }
+
+  // Need at least 2 targets
+  if (!result.HasTargets() || result.GetTargets().size() < 2) {
+    return Update(result, this->multiTagFallbackStrategy);
   }
 
   auto const targets = result.GetTargets();
@@ -408,7 +405,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
 
   // We should only do multi-tag if at least 2 tags (* 4 corners/tag)
   if (imagePoints.size() < 8) {
-    return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy);
+    return Update(result, this->multiTagFallbackStrategy);
   }
 
   // Output mats for results

photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp

@@ -335,7 +335,6 @@ PhotonPipelineResult PhotonCameraSim::Process(
     }
   }
 
-  heartbeatCounter++;
   return PhotonPipelineResult{
       PhotonPipelineMetadata{heartbeatCounter, 0,
                              units::microsecond_t{latency}.to<int64_t>(),
@@ -388,6 +387,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
   ts.cameraDistortionPublisher.Set(distortionView, ReceiveTimestamp);
 
   ts.heartbeatPublisher.Set(heartbeatCounter, ReceiveTimestamp);
+  heartbeatCounter++;
 
   ts.subTable->GetInstance().Flush();
 }

photon-lib/src/main/native/include/PhotonVersion.h

@@ -24,13 +24,12 @@
 
 #pragma once
 
-#include <regex>
-#include <string>
-
 namespace photon {
 namespace PhotonVersion {
 extern const char* versionString;
 extern const char* buildDate;
 extern const bool isRelease;
+extern const char* wpilibTargetVersion;
+extern const char* opencvTargetVersion;
 }  // namespace PhotonVersion
 }  // namespace photon

photon-lib/src/main/native/include/photon/PhotonCamera.h

@@ -28,6 +28,7 @@
 #include <string>
 #include <vector>
 
+#include <frc/Alert.h>
 #include <networktables/BooleanTopic.h>
 #include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
@@ -156,6 +157,14 @@ class PhotonCamera {
    */
   const std::string_view GetCameraName() const;
 
+  /**
+   * Returns whether the camera is connected and actively returning new data.
+   * Connection status is debounced.
+   *
+   * @return True if the camera is actively sending frame data, false otherwise.
+   */
+  bool IsConnected();
+
   using CameraMatrix = Eigen::Matrix<double, 3, 3>;
   using DistortionMatrix = Eigen::Matrix<double, 8, 1>;
 
@@ -203,18 +212,31 @@ class PhotonCamera {
   nt::BooleanPublisher driverModePublisher;
   nt::IntegerSubscriber ledModeSubscriber;
 
+  nt::IntegerSubscriber heartbeatSubscriber;
+
   nt::MultiSubscriber topicNameSubscriber;
 
   std::string path;
   std::string cameraName;
 
+  frc::Alert disconnectAlert;
+  frc::Alert timesyncAlert;
+
  private:
   units::second_t lastVersionCheckTime = 0_s;
   static bool VERSION_CHECK_ENABLED;
   inline static int InstanceCount = 0;
 
+  units::second_t prevTimeSyncWarnTime = 0_s;
+
+  int prevHeartbeatValue = -1;
+  units::second_t prevHeartbeatChangeTime = 0_s;
+
   void VerifyVersion();
 
+  void UpdateDisconnectAlert();
+  void CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result);
+
   std::vector<std::string> tablesThatLookLikePhotonCameras();
 };
 

photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h

@@ -204,6 +204,21 @@ class PhotonPoseEstimator {
 
   inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
 
+  /**
+   * Internal convenience method for using a fallback strategy for update().
+   * This should only be called after timestamp checks have been done by another
+   * update() overload.
+   *
+   * @param cameraResult The latest pipeline result from the camera
+   * @param strategy The pose strategy to use
+   * @return an EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
+                                           PoseStrategy strategy) {
+    return Update(result, std::nullopt, std::nullopt, strategy);
+  }
+
   std::optional<EstimatedRobotPose> Update(
       const PhotonPipelineResult& result,
       std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,

photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h

@@ -280,8 +280,8 @@ class SimCameraProperties {
   int resHeight;
   Eigen::Matrix<double, 3, 3> camIntrinsics;
   Eigen::Matrix<double, 8, 1> distCoeffs;
-  double avgErrorPx;
-  double errorStdDevPx;
+  double avgErrorPx{0};
+  double errorStdDevPx{0};
   units::second_t frameSpeed{0};
   units::second_t exposureTime{0};
   units::second_t avgLatency{0};

photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java

@@ -25,6 +25,9 @@
 package org.photonvision;
 
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assumptions.assumeTrue;
 import static org.photonvision.UnitTestUtils.waitForCondition;
 import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
@@ -33,8 +36,14 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.io.IOException;
+import java.util.Arrays;
+import java.util.List;
+import java.util.Optional;
 import java.util.stream.Stream;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.Assertions;
@@ -49,24 +58,38 @@ import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.jni.TimeSyncClient;
 import org.photonvision.jni.WpilibLoader;
 import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.targeting.PhotonPipelineMetadata;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 class PhotonCameraTest {
+    // A test-scoped, local-only NT instance
+    NetworkTableInstance inst = null;
+
     @BeforeAll
     public static void load_wpilib() {
         WpilibLoader.loadLibraries();
-
-        // See #1574 - test flakey, disabled until we address this
-        assumeTrue(false);
     }
 
     @BeforeEach
     public void setup() {
+        assertNull(inst);
+
         HAL.initialize(500, 0);
+
+        inst = NetworkTableInstance.create();
+        inst.stopClient();
+        inst.stopServer();
+        inst.startLocal();
+        SmartDashboard.setNetworkTableInstance(inst);
     }
 
     @AfterEach
     public void teardown() {
+        inst.close();
+        inst = null;
+
+        SimHooks.resumeTiming();
+
         HAL.shutdown();
     }
 
@@ -87,12 +110,10 @@ class PhotonCameraTest {
         load_wpilib();
         PhotonTargetingJniLoader.load();
 
-        HAL.initialize(500, 0);
+        inst.stopClient();
+        inst.startServer();
 
-        NetworkTableInstance.getDefault().stopClient();
-        NetworkTableInstance.getDefault().startServer();
-
-        var camera = new PhotonCamera("Arducam_OV2311_USB_Camera");
+        var camera = new PhotonCamera(inst, "Arducam_OV2311_USB_Camera");
         PhotonCamera.setVersionCheckEnabled(false);
 
         for (int i = 0; i < 5; i++) {
@@ -122,7 +143,7 @@ class PhotonCameraTest {
 
     private static Stream<Arguments> testNtOffsets() {
         return Stream.of(
-                // various initializaiton orders
+                // various initialization orders
                 Arguments.of(1, 10, 30, 30),
                 Arguments.of(10, 2, 30, 30),
                 Arguments.of(10, 10, 30, 30),
@@ -132,6 +153,38 @@ class PhotonCameraTest {
                 Arguments.of(1, 1, 30, 10));
     }
 
+    private void configureInstanceDataLoggers(NetworkTableInstance inst, String name) {
+        // Consumer<NetworkTableEvent> printEvent = (NetworkTableEvent e) -> {
+        //     if (e.is(Kind.kConnected)) {
+        //         System.out.println(name + ": Connected to " + e.connInfo);
+        //     }
+        //     if (e.is(Kind.kDisconnected)) {
+        //         System.out.println(name + ": Disconnected from " + e.connInfo);
+        //     }
+        //     if (e.is(Kind.kPublish)) {
+        //         System.out.println(name + ": Topic published: " + e.topicInfo);
+        //     }
+        //     if (e.is(Kind.kUnpublish)) {
+        //         System.out.println(name + ": Topic removed " + e.topicInfo);
+        //     }
+        //     if (e.is(Kind.kValueAll)) {
+        //         System.out.println(name + ": Value changed " + e.valueData);
+        //     }
+        //     if (e.is(Kind.kLogMessage)) {
+        //         System.out.println(name + ": LOG: " + e.logMessage);
+        //     }
+        // };
+
+        // inst.addConnectionListener(true, printEvent);
+        // inst.addListener(new String[]{""}, EnumSet.of(NetworkTableEvent.Kind.kTopic,
+        // NetworkTableEvent.Kind.kValueAll), printEvent);
+
+        var log = DataLogManager.getLog();
+        inst.startEntryDataLog(log, "", name + "_NT:");
+        inst.startConnectionDataLog(log, name + "_NTCONNECTION:");
+        System.out.println(name + ": Started logging to " + DataLogManager.getLogDir());
+    }
+
     /**
      * Try starting client before server and vice-versa, making sure that we never fail the version
      * check
@@ -140,9 +193,21 @@ class PhotonCameraTest {
     @MethodSource("testNtOffsets")
     public void testRestartingRobotAndCoproc(
             int robotStart, int coprocStart, int robotRestart, int coprocRestart) throws Throwable {
+        // See #1574 - test flakey, disabled until we address this
+        assumeTrue(false);
+
         var robotNt = NetworkTableInstance.create();
         var coprocNt = NetworkTableInstance.create();
 
+        configureInstanceDataLoggers(robotNt, "ROBOT 1");
+        configureInstanceDataLoggers(coprocNt, "COPROC 1");
+
+        // Don't need inst
+        inst.close();
+
+        // rename log
+        DataLogManager.start("logs", "testRestartingRobotAndCoproc.wpilog");
+
         robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
         coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
 
@@ -166,6 +231,7 @@ class PhotonCameraTest {
                 fakePhotonCoprocCam.close();
                 coprocNt.close();
                 coprocNt = NetworkTableInstance.create();
+                configureInstanceDataLoggers(coprocNt, "COPROC 2");
 
                 coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
 
@@ -180,6 +246,7 @@ class PhotonCameraTest {
 
                 robotNt.close();
                 robotNt = NetworkTableInstance.create();
+                configureInstanceDataLoggers(robotNt, "ROBOT 2");
                 robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
                 robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
             }
@@ -233,5 +300,94 @@ class PhotonCameraTest {
         coprocNt.close();
         robotNt.close();
         tspClient.stop();
+
+        DataLogManager.stop();
+    }
+
+    @Test
+    public void testAlerts() throws InterruptedException {
+        // GIVEN a fresh NT instance
+
+        var cameraName = "foobar";
+
+        // AND a photoncamera that is disconnected
+        var camera = new PhotonCamera(inst, cameraName);
+        assertFalse(camera.isConnected());
+
+        String disconnectedCameraString = "PhotonCamera '" + cameraName + "' is disconnected.";
+
+        // Loop to hit cases past first iteration
+        for (int i = 0; i < 10; i++) {
+            // WHEN we update the camera
+            camera.getAllUnreadResults();
+
+            // AND we tick SmartDashboard
+            SmartDashboard.updateValues();
+
+            // The alert state will be set (hard-coded here)
+            assertTrue(
+                    Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
+                            .anyMatch(it -> it.equals(disconnectedCameraString)));
+
+            Thread.sleep(20);
+        }
+
+        // GIVEN a simulated camera
+        var sim = new PhotonCameraSim(camera);
+        // AND a result with a timeSinceLastPong in the past
+        PhotonPipelineResult noPongResult =
+                new PhotonPipelineResult(
+                        new PhotonPipelineMetadata(
+                                1, 2, 3, 10 * 1000000 // 10 seconds -> us since last pong
+                                ),
+                        List.of(),
+                        Optional.empty());
+
+        // Loop to hit cases past first iteration
+        for (int i = 0; i < 10; i++) {
+            // AND a PhotonCamera with a "new" result
+            sim.submitProcessedFrame(noPongResult);
+
+            // WHEN we update the camera
+            camera.getAllUnreadResults();
+
+            // AND we tick SmartDashboard
+            SmartDashboard.updateValues();
+
+            // THEN the camera isn't disconnected
+            assertTrue(
+                    Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
+                            .noneMatch(it -> it.equals(disconnectedCameraString)));
+            // AND the alert string looks like a timesync warning
+            assertTrue(
+                    Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
+                                    .filter(it -> it.contains("is not connected to the TimeSyncServer"))
+                                    .count()
+                            == 1);
+
+            Thread.sleep(20);
+        }
+
+        final double HEARTBEAT_TIMEOUT = 0.5;
+
+        // GIVEN a PhotonCamera provided new results
+        SimHooks.pauseTiming();
+        sim.submitProcessedFrame(noPongResult);
+        camera.getAllUnreadResults();
+        // AND in a connected state
+        assertTrue(camera.isConnected());
+
+        // WHEN we wait the timeout
+        SimHooks.stepTiming(HEARTBEAT_TIMEOUT * 1.5);
+
+        // THEN the camera will not be connected
+        assertFalse(camera.isConnected());
+
+        // WHEN we then provide new results
+        SimHooks.stepTiming(0.02);
+        sim.submitProcessedFrame(noPongResult);
+        camera.getAllUnreadResults();
+        // THEN the camera will not be connected
+        assertTrue(camera.isConnected());
     }
 }

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -24,32 +24,62 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import org.junit.jupiter.api.AfterAll;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
+import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.estimation.TargetModel;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.WpilibLoader;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionTargetSim;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PhotonPipelineMetadata;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.PnpResult;
 import org.photonvision.targeting.TargetCorner;
 
 class PhotonPoseEstimatorTest {
     static AprilTagFieldLayout aprilTags;
 
     @BeforeAll
-    public static void init() {
+    public static void init() throws UnsatisfiedLinkError, IOException {
+        if (!WpilibLoader.loadLibraries()) {
+            fail();
+        }
+        if (!PhotonTargetingJniLoader.load()) {
+            fail();
+        }
+
+        HAL.initialize(1000, 0);
+
         List<AprilTag> tagList = new ArrayList<>(2);
         tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
         tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
@@ -58,6 +88,11 @@ class PhotonPoseEstimatorTest {
         aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
     }
 
+    @AfterAll
+    public static void teardown() {
+        HAL.shutdown();
+    }
+
     @Test
     void testLowestAmbiguityStrategy() {
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -488,6 +523,68 @@ class PhotonPoseEstimatorTest {
         assertEquals(1, pose.getZ(), .01);
     }
 
+    @Test
+    void pnpDistanceTrigSolve() {
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        PhotonCameraSim cameraOneSim =
+                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG());
+
+        List<VisionTargetSim> simTargets =
+                aprilTags.getTags().stream()
+                        .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
+                        .toList();
+
+        /* Compound Rolled + Pitched + Yaw */
+
+        Transform3d compoundTestTransform =
+                new Transform3d(
+                        -Units.inchesToMeters(12),
+                        -Units.inchesToMeters(11),
+                        3,
+                        new Rotation3d(
+                                Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60)));
+
+        var estimator =
+                new PhotonPoseEstimator(
+                        aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+
+        /* this is the real pose of the robot base we test against */
+        var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
+        PhotonPipelineResult result =
+                cameraOneSim.process(
+                        1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
+
+        estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+
+        var estimatedPose = estimator.update(result);
+        var pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(realPose.getX(), pose.getX(), .01);
+        assertEquals(realPose.getY(), pose.getY(), .01);
+        assertEquals(0.0, pose.getZ(), .01);
+
+        /* Straight on */
+
+        Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, new Rotation3d(0, 0, 0));
+
+        estimator.setRobotToCameraTransform(straightOnTestTransform);
+
+        /* Pose to compare with */
+        realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
+        result =
+                cameraOneSim.process(
+                        1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
+
+        estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
+
+        estimatedPose = estimator.update(result);
+        pose = estimatedPose.get().estimatedPose;
+
+        assertEquals(realPose.getX(), pose.getX(), .01);
+        assertEquals(realPose.getY(), pose.getY(), .01);
+        assertEquals(0.0, pose.getZ(), .01);
+    }
+
     @Test
     void cacheIsInvalidated() {
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -645,6 +742,175 @@ class PhotonPoseEstimatorTest {
         assertEquals(2.15, pose.getZ(), .01);
     }
 
+    @Test
+    void testMultiTagOnRioFallback() {
+        PhotonCameraInjector camera = new PhotonCameraInjector();
+        camera.result =
+                new PhotonPipelineResult(
+                        0,
+                        11 * 1_000_000,
+                        1_100_000,
+                        1024,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        -1,
+                                        -1,
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
+        estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update(camera.result);
+        Pose3d pose = estimatedPose.get().estimatedPose;
+        // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
+        assertAll(
+                () -> assertEquals(11, estimatedPose.get().timestampSeconds),
+                () -> assertEquals(1, pose.getX(), 1e-9),
+                () -> assertEquals(3, pose.getY(), 1e-9),
+                () -> assertEquals(2, pose.getZ(), 1e-9));
+    }
+
+    @Test
+    public void testConstrainedPnpOneTag() {
+        var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
+        var cameraMat =
+                MatBuilder.fill(
+                        Nat.N3(),
+                        Nat.N3(),
+                        399.37500000000006,
+                        0,
+                        319.5,
+                        0,
+                        399.16666666666674,
+                        239.5,
+                        0,
+                        0,
+                        1);
+
+        /*
+         * Ground truth:
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0.
+         * 31064262190452635
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0.
+         * 24478552235412665
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0.
+         * 0836470779150917
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0.
+         * 914649865171567
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3.
+         * 191446451763934
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4.
+         * 44396966389316
+         * 29.989279,NT:/photonvision/YOUR CAMERA
+         * NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0.
+         * 4995793771070878
+         */
+        List<TargetCorner> corners8 =
+                List.of(
+                        new TargetCorner(98.09875447066685, 331.0093220119495),
+                        new TargetCorner(122.20226758624413, 335.50083894738486),
+                        new TargetCorner(127.17118732489361, 313.81406314178633),
+                        new TargetCorner(104.28543773760417, 309.6516557438994));
+
+        var result =
+                new PhotonPipelineResult(
+                        new PhotonPipelineMetadata(10000, 2000, 1, 100),
+                        List.of(
+                                new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
+                        Optional.of(
+                                new MultiTargetPNPResult(
+                                        new PnpResult(
+                                                new Transform3d(
+                                                        // From ground truth
+                                                        new Translation3d(
+                                                                3.1665557336121353, 4.430673446050584, 0.48678786477534686),
+                                                        new Rotation3d(
+                                                                new Quaternion(
+                                                                        0.3132532247418243,
+                                                                        0.24722671090692333,
+                                                                        -0.08413452932300695,
+                                                                        0.9130568172784148))),
+                                                0.1),
+                                        new ArrayList<Short>(8))));
+
+        final double camPitch = Units.degreesToRadians(30.0);
+        final Transform3d kRobotToCam =
+                new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP,
+                        kRobotToCam);
+
+        estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
+
+        Optional<EstimatedRobotPose> estimatedPose =
+                estimator.update(
+                        result,
+                        Optional.of(cameraMat),
+                        Optional.of(distortion),
+                        Optional.of(new ConstrainedSolvepnpParams(true, 0)));
+        Pose3d pose = estimatedPose.get().estimatedPose;
+        System.out.println(pose);
+    }
+
+    @Test
+    void testConstrainedPnpEmptyCase() {
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP,
+                        Transform3d.kZero);
+        PhotonPipelineResult result = new PhotonPipelineResult();
+        var estimate = estimator.update(result);
+        assertEquals(estimate, Optional.empty());
+    }
+
     private static class PhotonCameraInjector extends PhotonCamera {
         public PhotonCameraInjector() {
             super("Test");

photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java

@@ -24,6 +24,8 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
 import java.util.regex.Matcher;
 import java.util.regex.Pattern;
 import org.junit.jupiter.api.Assertions;
@@ -56,4 +58,9 @@ public class PhotonVersionTest {
         Assertions.assertFalse(versionMatches("", "v2021.1.6"));
         Assertions.assertFalse(versionMatches("v2021.1.6", ""));
     }
+
+    @Test
+    public void testNominalDeps() {
+        assertDoesNotThrow(PhotonCamera::verifyDependencies);
+    }
 }

photon-lib/src/test/java/org/photonvision/UnitTestUtils.java

@@ -50,10 +50,10 @@ public class UnitTestUtils {
     }
 
     static PhotonPipelineResult waitForSequenceNumber(PhotonCamera camera, int seq) {
-        assertTrue(camera.heartbeatEntry.getTopic().getHandle() != 0);
+        assertTrue(camera.heartbeatSubscriber.getTopic().getHandle() != 0);
 
         System.out.println(
-                "Waiting for seq=" + seq + " on " + camera.heartbeatEntry.getTopic().getName());
+                "Waiting for seq=" + seq + " on " + camera.heartbeatSubscriber.getTopic().getName());
         // wait up to 1 second for a new result
         for (int i = 0; i < 100; i++) {
             var res = camera.getLatestResult();

photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java

@@ -33,6 +33,7 @@ import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.cscore.OpenCvLoader;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -57,7 +58,6 @@ import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
 import org.junit.jupiter.params.provider.ValueSource;
-import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.jni.PhotonTargetingJniLoader;
@@ -84,7 +84,7 @@ class VisionSystemSimTest {
             fail(e);
         }
 
-        OpenCVHelp.forceLoadOpenCV();
+        OpenCvLoader.forceStaticLoad();
 
         // See #1574 - test flakey, disabled until we address this
         assumeTrue(false);

photon-lib/src/test/native/cpp/PhotonCameraTest.cpp

@@ -22,12 +22,19 @@
  * SOFTWARE.
  */
 
+#include <fmt/ranges.h>
 #include <gtest/gtest.h>
 #include <hal/HAL.h>
 #include <net/TimeSyncClient.h>
 #include <net/TimeSyncServer.h>
+#include <photon/PhotonCamera.h>
+#include <photon/simulation/PhotonCameraSim.h>
 
-#include "photon/PhotonCamera.h"
+#include <string>
+#include <vector>
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <networktables/NetworkTableInstance.h>
 
 TEST(TimeSyncProtocolTest, Smoketest) {
   using namespace wpi::tsp;
@@ -52,3 +59,77 @@ TEST(TimeSyncProtocolTest, Smoketest) {
 
   client.Stop();
 }
+
+TEST(PhotonCameraTest, Alerts) {
+  using frc::SmartDashboard;
+
+  // GIVEN a local-only NT instance
+  auto inst = nt::NetworkTableInstance::GetDefault();
+  inst.StopClient();
+  inst.StopServer();
+  inst.StartLocal();
+  // (We can't create our own instance, SmartDashboard will always use the
+  // default)
+
+  const std::string cameraName = "foobar";
+
+  // AND a PhotonCamera that is disconnected
+  photon::PhotonCamera camera(inst, cameraName);
+  EXPECT_FALSE(camera.IsConnected());
+  std::string disconnectedCameraString =
+      "PhotonCamera '" + cameraName + "' is disconnected.";
+
+  // Loop to hit cases past first iteration
+  for (int i = 0; i < 10; i++) {
+    // WHEN we update the camera
+    camera.GetAllUnreadResults();
+    // AND we tick SmartDashboard
+    SmartDashboard::UpdateValues();
+
+    // The alert state will be set (hard-coded here)
+    auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {});
+    EXPECT_TRUE(
+        std::any_of(alerts.begin(), alerts.end(),
+                    [&disconnectedCameraString](const std::string& alert) {
+                      return alert == disconnectedCameraString;
+                    }));
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+
+  // GIVEN a simulated camera
+  photon::PhotonCameraSim sim(&camera);
+  // AND a result with a timeSinceLastPong in the past
+  photon::PhotonPipelineMetadata metadata{3, 1, 2, 10 * 1000000};
+  photon::PhotonPipelineResult noPongResult{
+      metadata, std::vector<photon::PhotonTrackedTarget>{}, std::nullopt};
+
+  // Loop to hit cases past first iteration
+  for (int i = 0; i < 10; i++) {
+    // AND a PhotonCamera with a "new" result
+    sim.SubmitProcessedFrame(noPongResult);
+    // WHEN we update the camera
+    camera.GetAllUnreadResults();
+    // AND we tick SmartDashboard
+    SmartDashboard::UpdateValues();
+
+    // THEN the camera isn't disconnected
+    auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {});
+    fmt::println("{}:{}: saw alerts: {}", __FILE__, __LINE__, alerts);
+    EXPECT_TRUE(
+        std::none_of(alerts.begin(), alerts.end(),
+                     [&disconnectedCameraString](const std::string& alert) {
+                       return alert == disconnectedCameraString;
+                     }));
+
+    // AND the alert string looks like a timesync warning
+    EXPECT_EQ(
+        1, std::count_if(
+               alerts.begin(), alerts.end(), [](const std::string& alert) {
+                 return alert.find("is not connected to the TimeSyncServer") !=
+                        std::string::npos;
+               }));
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+}

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -419,6 +419,49 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
 
   EXPECT_FALSE(estimatedPose);
 }
+
+TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.7, corners, detectedCorners},
+      photon::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {photon::PhotonPipelineResult{
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
+
+  photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
+                                        frc::Transform3d{});
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
+}
+
 TEST(PhotonPoseEstimatorTest, CopyResult) {
   std::vector<photon::PhotonTrackedTarget> targets{};
 

photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp

@@ -509,6 +509,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
 
   auto camResults = camera.GetLatestResult();
   auto targetSpan = camResults.GetTargets();
+
+  // We need to see at least one target
+  ASSERT_GT(targetSpan.size(), static_cast<size_t>(0));
+
   std::vector<photon::PhotonTrackedTarget> targets;
   for (photon::PhotonTrackedTarget tar : targetSpan) {
     targets.push_back(tar);

photon-serde/README.md

@@ -14,7 +14,7 @@ Like Rosmsg. But worse.
 
 The code for a single type is split across 3 files. Let's look at PnpResult:
 - [The struct definition](src/struct/pnpresult_struct.h): This is the data the object holds. Auto-generated. The data this object holds can be primitives or other, fully-deserialized types (like Vec2)
-- [The user class](src/targeting/pnpresult_struct.h): This is the fully-deserialized PnpResult type. This contains extra functions users might need to expose like `Amgiguity`, or other computed helper things.
+- [The user class](src/targeting/pnpresult_struct.h): This is the fully-deserialized PnpResult type. This contains extra functions users might need to expose like `Ambiguity`, or other computed helper things.
 - [The serde interface](src/serde/pnpresult_struct.h): This is a template specialization for converting the user class to/from bytes
 
 ## Prior art

photon-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -29,7 +29,6 @@ import java.nio.file.Paths;
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.Optional;
-import java.util.regex.Pattern;
 import javax.imageio.ImageIO;
 import org.apache.commons.io.FileUtils;
 import org.opencv.core.Mat;
@@ -432,7 +431,7 @@ public class RequestHandler {
             var tempPath2 = Files.createTempFile("photonvision-kernelogs", ".txt");
             // In the command below:
             // dmesg = output all kernel logs since current boot
-            // cat /var/log/kern.log = output all kernal logs since first boot
+            // cat /var/log/kern.log = output all kernel logs since first boot
             shell.executeBashCommand(
                     "journalctl -u photonvision.service > "
                             + tempPath.toAbsolutePath()
@@ -467,8 +466,8 @@ public class RequestHandler {
             }
         } catch (IOException e) {
             ctx.status(500);
-            ctx.result("There was an error while exporting journactl logs");
-            logger.error("There was an error while exporting journactl logs", e);
+            ctx.result("There was an error while exporting journalctl logs");
+            logger.error("There was an error while exporting journalctl logs", e);
         }
     }
 
@@ -573,24 +572,9 @@ public class RequestHandler {
             }
 
             // verify naming convention
-            // this check will need to be modified if different model types are added
 
-            Pattern modelPattern = Pattern.compile("^[a-zA-Z0-9]+-\\d+-\\d+-yolov[58][a-z]*\\.rknn$");
-
-            Pattern labelsPattern =
-                    Pattern.compile("^[a-zA-Z0-9]+-\\d+-\\d+-yolov[58][a-z]*-labels\\.txt$");
-
-            if (!modelPattern.matcher(modelFile.filename()).matches()
-                    || !labelsPattern.matcher(labelsFile.filename()).matches()
-                    || !(modelFile
-                            .filename()
-                            .substring(0, modelFile.filename().indexOf("-"))
-                            .equals(labelsFile.filename().substring(0, labelsFile.filename().indexOf("-"))))) {
-                ctx.status(400);
-                ctx.result("The uploaded files were not named correctly.");
-                logger.error("The uploaded object detection model files were not named correctly.");
-                return;
-            }
+            // throws IllegalArgumentException if the model name is invalid
+            NeuralNetworkModelManager.verifyRKNNNames(modelFile.filename(), labelsFile.filename());
 
             // TODO move into neural network manager
 

photon-server/src/main/resources/models/algaeV1-640-640-yolov8n-labels.txt

@@ -0,0 +1 @@
+algae

photon-server/src/main/resources/models/note-640-640-yolov5s-labels.txt

@@ -1 +0,0 @@
-note

photon-targeting/build.gradle

@@ -3,6 +3,7 @@ ext {
 }
 
 apply plugin: 'cpp'
+apply plugin: 'c'
 apply plugin: 'google-test-test-suite'
 apply plugin: 'edu.wpi.first.NativeUtils'
 apply plugin: 'edu.wpi.first.WpilibTools'
@@ -40,6 +41,12 @@ model {
     components {
         "${nativeName}"(NativeLibrarySpec) {
             sources {
+                c {
+                    source {
+                        srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp", "$buildDir/generated/native/cpp"
+                        include '**/*.c'
+                    }
+                }
                 cpp {
                     source {
                         srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp", 'src/generated/main/native/cpp'

photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java

@@ -121,7 +121,7 @@ public enum Platform {
     }
 
     public static boolean isRK3588() {
-        return Platform.isOrangePi() || Platform.isCoolPi4b();
+        return Platform.isOrangePi() || Platform.isCoolPi4b() || Platform.isRock5C();
     }
 
     public static boolean isRaspberryPi() {
@@ -217,7 +217,7 @@ public enum Platform {
                 return LINUX_32;
             } else if (OS_ARCH.equals("aarch64") || OS_ARCH.equals("arm64")) {
                 // TODO - os detection needed?
-                if (isOrangePi()) {
+                if (isRK3588()) {
                     return LINUX_RK3588_64;
                 } else {
                     return LINUX_AARCH64;
@@ -243,6 +243,10 @@ public enum Platform {
         return fileHasText("/proc/device-tree/model", "Orange Pi 5");
     }
 
+    private static boolean isRock5C() {
+        return fileHasText("/proc/device-tree/model", "ROCK 5C");
+    }
+
     private static boolean isCoolPi4b() {
         return fileHasText("/proc/device-tree/model", "CoolPi 4B");
     }

photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java

@@ -17,7 +17,7 @@
 
 package org.photonvision.estimation;
 
-import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.OpenCvLoader;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -54,14 +54,12 @@ public final class OpenCVHelp {
     private static final Rotation3d NWU_TO_EDN;
     private static final Rotation3d EDN_TO_NWU;
 
-    // Creating a cscore object is sufficient to load opencv, per
-    // https://www.chiefdelphi.com/t/unsatisfied-link-error-when-simulating-java-robot-code-using-opencv/426731/4
-    private static CvSink dummySink = null;
-
+    /**
+     * @deprecated Replaced by {@link OpenCvLoader#forceStaticLoad()}
+     */
+    @Deprecated(since = "2025", forRemoval = true)
     public static void forceLoadOpenCV() {
-        if (dummySink != null) return;
-        dummySink = new CvSink("ignored");
-        dummySink.close();
+        OpenCvLoader.forceStaticLoad();
     }
 
     static {

photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java

@@ -19,8 +19,13 @@ package org.photonvision.estimation;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.cscore.OpenCvLoader;
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.*;
@@ -29,7 +34,12 @@ import java.util.List;
 import java.util.Objects;
 import java.util.Optional;
 import java.util.stream.Collectors;
+import org.ejml.simple.SimpleMatrix;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
+import org.photonvision.jni.ConstrainedSolvepnpJni;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.PnpResult;
 import org.photonvision.targeting.TargetCorner;
@@ -96,6 +106,8 @@ public class VisionEstimation {
         if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
             return Optional.empty();
         }
+        OpenCvLoader.forceStaticLoad();
+
         Point[] points = OpenCVHelp.cornersToPoints(corners);
 
         // single-tag pnp
@@ -132,4 +144,140 @@ public class VisionEstimation {
                             camToOrigin.get().altReprojErr));
         }
     }
+
+    /**
+     * Performs constrained solvePNP using 3d-2d point correspondences of visible AprilTags to
+     * estimate the field-to-camera transformation.
+     *
+     * <p><b>Note:</b> The returned transformation is from the field origin to the robot drivebase
+     *
+     * @param cameraMatrix The camera intrinsics matrix in standard opencv form
+     * @param distCoeffs The camera distortion matrix in standard opencv form
+     * @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
+     * @param robot2camera The {@link Transform3d} from the robot odometry frame to the camera optical
+     *     frame
+     * @param robotPoseSeed An initial guess at robot pose, refined via optimizaiton. Better guesses
+     *     will converge faster.
+     * @param tagLayout The known tag layout on the field
+     * @param tagModel The physical size of the AprilTags
+     * @param headingFree If heading is completely free, or if our measured gyroθ is taken into
+     *     account
+     * @param gyroθ If headingFree is false, the best estimate at robot yaw. Excursions from this are
+     *     penalized in our cost function.
+     * @param gyroErrorScaleFac A relative weight for gyro heading excursions against tag corner
+     *     reprojection error.
+     * @return The transformation that maps the field origin to the camera pose. Ensure the {@link
+     *     PnpResult} are present before utilizing them.
+     */
+    public static Optional<PnpResult> estimateRobotPoseConstrainedSolvepnp(
+            Matrix<N3, N3> cameraMatrix,
+            Matrix<N8, N1> distCoeffs,
+            List<PhotonTrackedTarget> visTags,
+            Transform3d robot2camera,
+            Pose3d robotPoseSeed,
+            AprilTagFieldLayout tagLayout,
+            TargetModel tagModel,
+            boolean headingFree,
+            Rotation2d gyroθ,
+            double gyroErrorScaleFac) {
+        if (tagLayout == null
+                || visTags == null
+                || tagLayout.getTags().isEmpty()
+                || visTags.isEmpty()) {
+            return Optional.empty();
+        }
+
+        var corners = new ArrayList<TargetCorner>();
+        var knownTags = new ArrayList<AprilTag>();
+        // ensure these are AprilTags in our layout
+        for (var tgt : visTags) {
+            int id = tgt.getFiducialId();
+            tagLayout
+                    .getTagPose(id)
+                    .ifPresent(
+                            pose -> {
+                                knownTags.add(new AprilTag(id, pose));
+                                corners.addAll(tgt.getDetectedCorners());
+                            });
+        }
+        if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
+            return Optional.empty();
+        }
+        OpenCvLoader.forceStaticLoad();
+
+        Point[] points = OpenCVHelp.cornersToPoints(corners);
+
+        // Undistort
+        {
+            MatOfPoint2f temp = new MatOfPoint2f();
+            MatOfDouble cameraMatrixMat = new MatOfDouble();
+            MatOfDouble distCoeffsMat = new MatOfDouble();
+            OpenCVHelp.matrixToMat(cameraMatrix.getStorage()).assignTo(cameraMatrixMat);
+            OpenCVHelp.matrixToMat(distCoeffs.getStorage()).assignTo(distCoeffsMat);
+
+            temp.fromArray(points);
+            Calib3d.undistortImagePoints(temp, temp, cameraMatrixMat, distCoeffsMat);
+            points = temp.toArray();
+
+            temp.release();
+            cameraMatrixMat.release();
+            distCoeffsMat.release();
+        }
+
+        // Rotate from wpilib to opencv camera CS
+        var robot2cameraBase =
+                MatBuilder.fill(Nat.N4(), Nat.N4(), 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1);
+        var robotToCamera = robot2camera.toMatrix().times(robot2cameraBase);
+
+        // Where we saw corners
+        var point_observations = new SimpleMatrix(2, points.length);
+        for (int i = 0; i < points.length; i++) {
+            point_observations.set(0, i, points[i].x);
+            point_observations.set(1, i, points[i].y);
+        }
+
+        // Affine corner locations
+        var objectTrls = new ArrayList<Translation3d>();
+        for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
+        var field2points = new SimpleMatrix(4, points.length);
+        for (int i = 0; i < objectTrls.size(); i++) {
+            field2points.set(0, i, objectTrls.get(i).getX());
+            field2points.set(1, i, objectTrls.get(i).getY());
+            field2points.set(2, i, objectTrls.get(i).getZ());
+            field2points.set(3, i, 1);
+        }
+
+        // fx fy cx cy
+        double[] cameraCal =
+                new double[] {
+                    cameraMatrix.get(0, 0),
+                    cameraMatrix.get(1, 1),
+                    cameraMatrix.get(0, 2),
+                    cameraMatrix.get(1, 2),
+                };
+
+        var guess2 = robotPoseSeed.toPose2d();
+
+        var ret =
+                ConstrainedSolvepnpJni.do_optimization(
+                        headingFree,
+                        knownTags.size(),
+                        cameraCal,
+                        robotToCamera.getData(),
+                        new double[] {
+                            guess2.getX(), guess2.getY(), guess2.getRotation().getRadians(),
+                        },
+                        field2points.getDDRM().getData(),
+                        point_observations.getDDRM().getData(),
+                        gyroθ.getRadians(),
+                        gyroErrorScaleFac);
+
+        if (ret == null) {
+            return Optional.empty();
+        } else {
+            var pnpresult = new PnpResult();
+            pnpresult.best = new Transform3d(new Transform2d(ret[0], ret[1], new Rotation2d(ret[2])));
+            return Optional.of(pnpresult);
+        }
+    }
 }

photon-targeting/src/main/java/org/photonvision/jni/ConstrainedSolvepnpJni.java

@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.jni;
+
+public class ConstrainedSolvepnpJni {
+    public static native double[] do_optimization(
+            boolean heading_free,
+            int nTags,
+            double[] cameraCal,
+            double[] robot2camera,
+            double[] x_guess,
+            double[] field2points,
+            double[] point_observations,
+            double gyro_θ,
+            double gyro_error_scale_fac);
+}

photon-targeting/src/main/java/org/photonvision/jni/WpilibLoader.java

@@ -17,10 +17,6 @@
 
 package org.photonvision.jni;
 
-import java.io.IOException;
-
-import org.opencv.core.Core;
-
 import edu.wpi.first.apriltag.jni.AprilTagJNI;
 import edu.wpi.first.cscore.CameraServerJNI;
 import edu.wpi.first.cscore.OpenCvLoader;
@@ -30,9 +26,12 @@ import edu.wpi.first.net.WPINetJNI;
 import edu.wpi.first.networktables.NetworkTablesJNI;
 import edu.wpi.first.util.CombinedRuntimeLoader;
 import edu.wpi.first.util.WPIUtilJNI;
+import java.io.IOException;
+import org.opencv.core.Core;
 
 public class WpilibLoader {
     private static boolean has_loaded = false;
+
     public static boolean loadLibraries() {
         if (has_loaded) return true;
 
@@ -45,9 +44,11 @@ public class WpilibLoader {
         WPIMathJNI.Helper.setExtractOnStaticLoad(false);
         AprilTagJNI.Helper.setExtractOnStaticLoad(false);
         try {
+            // Need to load wpiutil first before checking if the MSVC runtime is valid
+            CombinedRuntimeLoader.loadLibraries(WpilibLoader.class, "wpiutiljni");
+            WPIUtilJNI.checkMsvcRuntime();
             CombinedRuntimeLoader.loadLibraries(
                     WpilibLoader.class,
-                    "wpiutiljni",
                     "wpimathjni",
                     "ntcorejni",
                     "wpinetjni",
@@ -56,7 +57,6 @@ public class WpilibLoader {
                     "apriltagjni");
 
             CombinedRuntimeLoader.loadLibraries(WpilibLoader.class, Core.NATIVE_LIBRARY_NAME);
-
             has_loaded = true;
         } catch (IOException e) {
             e.printStackTrace();

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -164,7 +164,7 @@ public class PhotonPipelineResult
 
     /**
      * Returns the estimated time the frame was taken, in the Time Sync Server's time base (nt::Now).
-     * This is calculated using the estiamted offset between Time Sync Server time and local time. The
+     * This is calculated using the estimated offset between Time Sync Server time and local time. The
      * robot shall run a server, so the offset shall be 0.
      *
      * @return The timestamp in seconds

photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp

@@ -57,6 +57,31 @@ static void ClientLoggerFunc(unsigned int level, const char* file,
              line);
 }
 
+void wpi::tsp::TimeSyncClient::UpdateStatistics(uint64_t pong_local_time,
+                                                wpi::tsp::TspPing ping,
+                                                wpi::tsp::TspPong pong) {
+  // when time = send_time+rtt2/2, server time = server time
+  // server time = local time + offset
+  // offset = (server time - local time) = (server time) - (send_time +
+  // rtt2/2)
+  auto rtt2 = pong_local_time - ping.client_time;
+  int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
+
+  auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs);
+
+  // wpi::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS",
+  // rtt2,
+  //              serverTimeOffsetUs, filtered);
+
+  {
+    std::lock_guard lock{m_offsetMutex};
+    m_metadata.offset = filtered;
+    m_metadata.rtt2 = rtt2;
+    m_metadata.pongsReceived++;
+    m_metadata.lastPongTime = pong_local_time;
+  }
+}
+
 void wpi::tsp::TimeSyncClient::Tick() {
   // wpi::println("wpi::tsp::TimeSyncClient::Tick");
   // Regardless of if we've gotten a pong back yet, we'll ping again. this is
@@ -122,28 +147,9 @@ void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes,
     return;
   }
 
-  // when time = send_time+rtt2/2, server time = server time
-  // server time = local time + offset
-  // offset = (server time - local time) = (server time) - (send_time +
-  // rtt2/2)
-  auto rtt2 = pong_local_time - ping.client_time;
-  int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
+  UpdateStatistics(pong_local_time, ping, pong);
 
-  auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs);
-
-  // wpi::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS",
-  // rtt2,
-  //              serverTimeOffsetUs, filtered);
-
-  {
-    std::lock_guard lock{m_offsetMutex};
-    m_metadata.offset = filtered;
-    m_metadata.rtt2 = rtt2;
-    m_metadata.pongsReceived++;
-    m_metadata.lastPongTime = pong_local_time;
-  }
-
-  using std::cout;
+  // using std::cout;
   // wpi::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2,
   //              serverTimeOffsetUs);
   // wpi::println("Estimated server time {} s",