[TSP] Move Bind() to Start (#1538)

Fixes UB with static init. Turns out starting threads in static init doesn't work on windows.

.github/workflows/build.yml

@@ -48,13 +48,13 @@ jobs:
           fetch-depth: 0
       - name: Fetch tags
         run: git fetch --tags --force
-      - name: Install RoboRIO Toolchain
-        run: ./gradlew installRoboRioToolchain
       - name: Install Java 17
         uses: actions/setup-java@v4
         with:
           java-version: 17
           distribution: temurin
+      - name: Install RoboRIO Toolchain
+        run: ./gradlew installRoboRioToolchain
       # Need to publish to maven local first, so that C++ sim can pick it up
       # Still haven't figured out how to make the vendordep file be copied before trying to build examples
       - name: Publish photonlib to maven local
@@ -85,7 +85,7 @@ jobs:
       - name: Fetch tags
         run: git fetch --tags --force
       - name: Install Java 17
-        uses: actions/setup-java@v3
+        uses: actions/setup-java@v4
         with:
           java-version: 17
           distribution: temurin
@@ -115,6 +115,10 @@ jobs:
       - uses: actions/setup-python@v5
         with:
           python-version: '3.11'
+      - name: Install graphviz
+        run: |
+          sudo apt-get update
+          sudo apt-get -y install graphviz
       - name: Install dependencies
         working-directory: docs
         run: |
@@ -161,6 +165,7 @@ jobs:
       - run: |
           chmod +x gradlew
           ./gradlew photon-targeting:build photon-lib:build -i
+        name: Build with Gradle
       - run: ./gradlew photon-lib:publish photon-targeting:publish
         name: Publish
         env:
@@ -283,6 +288,9 @@ jobs:
           java-version: 17
           distribution: temurin
           architecture: ${{ matrix.architecture }}
+      - name: Install Arm64 Toolchain
+        run: ./gradlew installArm64Toolchain
+        if: ${{ (matrix.artifact-name) == 'LinuxArm64' }}
       - run: |
           rm -rf photon-server/src/main/resources/web/*
           mkdir -p photon-server/src/main/resources/web/docs
@@ -301,7 +309,7 @@ jobs:
           path: photon-server/src/main/resources/web/docs
       - run: |
           chmod +x gradlew
-          ./gradlew photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }}
+          ./gradlew photon-targeting:jar photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }}
         if: ${{ (matrix.arch-override != 'none') }}
       - run: |
           chmod +x gradlew
@@ -311,6 +319,10 @@ jobs:
         with:
           name: jar-${{ matrix.artifact-name }}
           path: photon-server/build/libs
+      - uses: actions/upload-artifact@v4
+        with:
+          name: photon-targeting_jar-${{ matrix.artifact-name }}
+          path: photon-targeting/build/libs
 
   run-smoketest-native:
     needs: [build-package]
@@ -344,7 +356,7 @@ jobs:
       - run: |
           sudo apt-get update
           sudo apt-get install --yes libcholmod3 liblapack3 libsuitesparseconfig5
-        if: ${{ (matrix.os) == 'ubuntu-latest' }}
+        if: ${{ (matrix.os) == 'ubuntu-22.04' }}
       # and actually run the jar
       - run: java -jar ${{ matrix.extraOpts }} *.jar --smoketest
         if: ${{ (matrix.os) != 'windows-latest' }}
@@ -439,7 +451,7 @@ jobs:
             image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2025.0.0-beta-6/photonvision_opi5pro.img.xz
             cpu: cortex-a8
             image_additional_mb: 1024
-          - os: ubuntu-latest
+          - os: ubuntu-22.04
             artifact-name: LinuxArm64
             image_suffix: orangepi5max
             image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2025.0.0-beta-6/photonvision_opi5max.img.xz

.github/workflows/lint-format.yml

@@ -57,13 +57,14 @@ jobs:
       - uses: actions/checkout@v3
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v3
+      - uses: actions/setup-java@v4
         with:
           java-version: 17
           distribution: temurin
       - run: |
           chmod +x gradlew
           ./gradlew spotlessCheck
+        name: Run spotless
 
   client-lint-format:
     name: "PhotonClient Lint and Formatting"

.github/workflows/photon-code-docs.yml

@@ -59,7 +59,7 @@ jobs:
     - name: Fetch tags
       run: git fetch --tags --force
     - name: Install Java 17
-      uses: actions/setup-java@v3
+      uses: actions/setup-java@v4
       with:
         java-version: 17
         distribution: temurin

.github/workflows/photonvision-docs.yml

@@ -26,6 +26,11 @@ jobs:
     - name: Install and upgrade pip
       run: python -m pip install --upgrade pip
 
+    - name: Install graphviz
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install graphviz
+
     - name: Install Python dependencies
       working-directory: docs
       run: |

.github/workflows/python.yml

@@ -21,7 +21,7 @@ on:
 
 jobs:
   buildAndDeploy:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
 
     steps:
       - name: Checkout code

.readthedocs.yaml

@@ -9,6 +9,8 @@ build:
   os: ubuntu-22.04
   tools:
     python: "3.11"
+  apt_packages:
+    - graphviz
   jobs:
     post_checkout:
       # Cancel building pull requests when there aren't changed in the docs directory or YAML file.

README.md

@@ -25,7 +25,7 @@ If you are interested in contributing code or documentation to the project, plea
 
 Gradle is used for all C++ and Java code, and NPM is used for the web UI. Instructions to compile PhotonVision yourself can be found [in our docs](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#compiling-instructions).
 
-You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the [`photonlib-java-examples`](photonlib-java-examples) and [`photonlib-cpp-examples`](photonlib-cpp-examples) subdirectories, respectively. Instructions for running these examples directly from the repo are found [in the docs](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
+You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the [`photonlib-java-examples`](photonlib-java-examples) and [`photonlib-cpp-examples`](photonlib-cpp-examples) subdirectories, respectively. Instructions for running these examples directly from the repo are found [in the docs](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#running-examples).
 
 ## Gradle Arguments
 
@@ -47,7 +47,7 @@ If you're cross-compiling, you'll need the wpilib toolchain installed. This can
 
 ## Out-of-Source Dependencies
 
-PhotonVision uses the following additonal out-of-source repositories for building code.
+PhotonVision uses the following additional out-of-source repositories for building code.
 
 - Base system images for Raspberry Pi & Orange Pi: https://github.com/PhotonVision/photon-image-modifier
 - C++ driver for Raspberry Pi CSI cameras: https://github.com/PhotonVision/photon-libcamera-gl-driver

build.gradle

@@ -4,11 +4,10 @@ plugins {
     id "java"
     id "cpp"
     id "com.diffplug.spotless" version "6.24.0"
-    id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
-    id "edu.wpi.first.GradleRIO" version "2024.3.2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
     id 'edu.wpi.first.WpilibTools' version '1.3.0'
-    id 'com.google.protobuf' version '0.9.4' apply false
+    id 'com.google.protobuf' version '0.9.3' apply false
     id 'edu.wpi.first.GradleJni' version '1.1.0'
 }
 
@@ -31,14 +30,15 @@ ext.allOutputsFolder = file("$project.buildDir/outputs")
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2024.3.2"
+    wpilibVersion = "2025.1.1-beta-1"
     wpimathVersion = wpilibVersion
-    openCVversion = "4.8.0-2"
+    openCVYear = "2024"
+    openCVversion = "4.8.0-4"
     joglVersion = "2.4.0"
     javalinVersion = "5.6.2"
     libcameraDriverVersion = "dev-v2023.1.0-14-g787ab59"
     rknnVersion = "dev-v2024.0.1-4-g0db16ac"
-    frcYear = "2024"
+    frcYear = "2025"
     mrcalVersion = "dev-v2024.0.0-24-gc1efcf0";
 
 

docs/source/conf.py

@@ -37,6 +37,7 @@ extensions = [
     "sphinx_design",
     "myst_parser",
     "sphinx.ext.mathjax",
+    "sphinx.ext.graphviz",
 ]
 
 # Configure OpenGraph support

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

@@ -26,7 +26,7 @@ The built documentation is located at `docs/build/html/index.html` relative to t
 
 ## Docs Builds on Pull Requests
 
-Pre-merge builds of docs can be found at: `https://photonvision-docs--PRNUMBER.org.readthedocs.build/en/PRNUMBER/index.html`. These docs are republished on every commit to a pull request made to PhotonVision/photonvision-docs. For example, PR 325 would have pre-merge documentation published to `https://photonvision-docs--325.org.readthedocs.build/en/325/index.html`. Additionally, the pull requrest will have a link directly to the pre-release build of the docs. This build only runs when there is a change to files in the docs sub-folder.
+Pre-merge builds of docs can be found at: `https://photonvision-docs--PRNUMBER.org.readthedocs.build/en/PRNUMBER/index.html`. These docs are republished on every commit to a pull request made to PhotonVision/photonvision-docs. For example, PR 325 would have pre-merge documentation published to `https://photonvision-docs--325.org.readthedocs.build/en/325/index.html`. Additionally, the pull request will have a link directly to the pre-release build of the docs. This build only runs when there is a change to files in the docs sub-folder.
 
 ## Style Guide
 

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

@@ -8,7 +8,7 @@ This section contains the build instructions from the source code available at [
 
 **Java Development Kit:**
 
- This project requires Java Development Kit (JDK) 17 to be compiled. This is the same Java version that comes with WPILib for 2025+. If you don't have this JDK with WPILib, you can follow the instructions to install JDK 17 for your platform [here](https://bell-sw.com/pages/downloads/#jdk-17-lts).
+ This project requires Java Development Kit (JDK) 17 to be compiled. This is the same Java version that comes with WPILib for 2025+. **Windows Users must use the JDK that ships with WPILib.** For other platforms, you can follow the instructions to install JDK 17 for your platform [here](https://bell-sw.com/pages/downloads/#jdk-17-lts).
 
 **Node JS:**
 

docs/source/docs/contributing/design-descriptions/index.md

@@ -3,4 +3,5 @@
 ```{toctree}
 :maxdepth: 1
 image-rotation
+time-sync
 ```

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

@@ -0,0 +1,111 @@
+# Time Synchronization Protocol Specification, Version 1.0
+
+Protocol Revision 1.0, 08/25/2024
+
+## Background
+
+In a distributed compute environment like robots, time synchronization between computers is increasingly important. Currently, [NetworkTables Version 4.1](https://github.com/wpilibsuite/allwpilib/blob/main/ntcore/doc/networktables4.adoc) provides support for time synchronization of clients with the NetworkTables server using binary PING/PONG messages sent over WebSockets. This approach, while fundamentally the same as is described in this memo, has demonstrated some opportunities for improvement:
+
+- PING/PONG messages are processed in the same queue as other NetworkTables messages. Depending on the underlying implementation and processor speed, this can incur message processing delays and increase client-calculated Round-Trip Time (RTT), and cause messages to arrive at the server timestamped in the future.
+- Messages use WebSockets over TCP for their transport layer. We don't need the robustness guarantees of TCP as our connection is stateless.
+
+For these reasons, a time synchronization solution separate from NetworkTables communication was desired. Architecture decisions made to address these issues are:
+
+- Use the User Datagram Protocol (UDP) transport layer, as we don't need the robustness guarantees afforded by TCP. As a Client, if a PING isn't replied to, we'll just try again at the start of the next PING window. As a bonus, we are free to use UDP port 5810 as NetworkTables only uses TCP Port 5810/5811 as of Version 4.1.
+- Use a separate thread from the current NetworkTables libUV runner.
+
+
+## Prior Art
+
+The [NetworkTables 4.1 timestamp synchronization](https://github.com/wpilibsuite/allwpilib/blob/main/ntcore/doc/networktables4.adoc#timestamps) approach, an implementation of [Cristian's Algorithm](https://en.wikipedia.org/wiki/Cristian%27s_algorithm). We also implement Cristian’s Algorithm.
+
+The [Precision Time Protocol](https://en.wikipedia.org/wiki/Precision_Time_Protocol#Synchronization) at it's core does something similar with Sync/Delay_Req/Delay_Resp. We do not have (guaranteed) access to hardware timestamping, but we utilize this PING/PONG pattern to estimate total round-trip time.
+
+
+## Roles
+
+```{graphviz}
+digraph CristianAlgorithm {
+    ratio=0.5;
+    bgcolor="transparent";
+
+    node [
+        fontcolor = "#e6e6e6",
+        style = filled,
+        color = "#e6e6e6",
+        fillcolor = "#333333"
+        fontsize=10;
+    ]
+
+    edge [
+        color = "#e6e6e6",
+        fontcolor = "#e6e6e6"
+        fontsize=10;
+    ]
+
+    rankdir=LR;
+    node [shape=box, style=filled, color=lightblue];
+
+    user_send [label="User Sends T1"];
+    server_receive [label="Server Receives T1"];
+    server_send [label="Server Sends T2"];
+    user_receive [label="User Receives T2"];
+    user_compute [label="User Computes Time"];
+
+    user_send -> server_receive [label="T1 (Request)"];
+    server_receive -> server_send [label="T1 received by server"];
+    server_send -> user_receive [label="T2 sent by server"];
+    user_receive -> user_compute [label="T2 received by user"];
+    user_compute -> user_send [label="Computed Time: T3 = T2 + (deltaT2 - deltaT1)/2"];
+}
+```
+
+Time Synchronization Protocol (TSP) participants can assume either a server role or a client role. The server role is responsible for listening for incoming time synchronization requests from clients and replying appropriately. The client role is responsible for sending "Ping" messages to the server and listening for "Pong" replies to estimate the offset between the server and client time bases.
+
+All time values shall use units of microseconds. The epoch of the time base this is measured against is unspecified.
+
+Clients shall periodically (e.g. every few seconds) send, in a manner that minimizes transmission delays, a **TSP Ping Message** that contains the client's current local time.
+
+When the server receives a **TSP Ping Message** from any client, it shall respond to the client, in a manner that minimizes transmission delays, with a **TSP Pong message** encoding a timestamp of its (the server's) current local time (in microseconds), and the client-provided data value.
+
+When the client receives a **TSP Pong Message** from the server, it shall verify that the `Client Local Time` corresponds to the currently in-flight TSP Ping message; if not, it shall drop this packet. The round trip time (RTT) shall be computed from the delta between the message's data value and the current local time.  If the RTT is less than that from previous measurements, the client shall use the timestamp in the message plus ½ the RTT as the server time equivalent to the current local time, and use this equivalence to compute server time base timestamps from local time for future messages.
+
+## Transport
+
+Communication between server and clients shall occur over the User Datagram Protocol (UDP) Port 5810.
+
+## 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.
+
+### TSP Ping
+
+| Offset | Format | Data | Notes |
+| ------ | ------ | ---- | ----- |
+| 0 | uint8 | Protocol version | This field shall always set to 1 (0b1) for TSP Version 1. |
+| 1 | uint8 | Message ID | This field shall always be set to 1 (0b1). |
+| 2 | uint64 | Client Local Time | The client's local time value, at the time this Ping message was sent. |
+
+### TSP Pong
+
+| Offset | Format | Data | Notes |
+| ------ | ------ | ---- | ----- |
+| 0 | uint8 | Protocol version | This field shall always set to 1 (0b1) for TSP Version 1.
+| 1 | uint8 | Message ID | This field shall always be set to 2 (0b2).
+| 2 | uint64 | Client Local Time | The client's local time value from the Ping message that this Pong is generated in response to.
+| 10 | uint64 | Server Local Time | The current time at the server, at the time this Pong message was sent.
+
+
+## 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:
+
+| Key | Type | Notes |
+| ------ | ------ | ---- | ----- |
+| offset_us | Integer | The time offset that, when added to the client's local clock, provides server time |
+| ping_tx_count | Integer | The total number of TSP Ping packets transmitted |
+| ping_rx_count | Integer | The total number of TSP Ping packets received |
+| pong_rx_time_us | Integer | The time, in client local time, that the last pong was received |
+| rtt2_us | Integer | The time in us from last complete (ping transmission to pong reception) |
+
+PhotonVision has chosen to publish to the sub-table `/photonvision/.timesync/{DEVICE_HOSTNAME}`. Future implementations of this protocol may decide to implement this as a structured data type.

docs/source/docs/installation/sw_install/raspberry-pi.md

@@ -7,7 +7,7 @@ A Pre-Built Raspberry Pi image is available for ease of installation.
 Download the latest release of the PhotonVision Raspberry image (.xz file) from the [releases page](https://github.com/PhotonVision/photonvision/releases). You do not need to extract the downloaded ZIP file.
 
 :::{note}
-Make sure you download the image that ends in '-RasberryPi.xz'.
+Make sure you download the image that ends in '-RaspberryPi.xz'.
 :::
 
 ## Flashing the Pi Image

docs/source/docs/programming/photonlib/controlling-led.md

@@ -14,7 +14,7 @@ You can control the vision LEDs of supported hardware via PhotonLib using the `s
       // Blink the LEDs.
       camera.SetLED(photonlib::VisionLEDMode::kBlink);
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         # Coming Soon!
 ```

docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md

@@ -62,7 +62,7 @@ You can also get the pipeline latency from a pipeline result using the `getLaten
       // Get the pipeline latency.
       units::second_t latency = result.GetLatency();
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         # Coming Soon!
 ```

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

@@ -24,7 +24,7 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all
       // The parameter for LoadAPrilTagLayoutField will be different depending on the game.
       frc::AprilTagFieldLayout aprilTagFieldLayout = frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo);
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         # Coming Soon!
 
@@ -81,7 +81,7 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s
       photonlib::RobotPoseEstimator estimator(
           aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         kRobotToCam = wpimath.geometry.Transform3d(
             wpimath.geometry.Translation3d(0.5, 0.0, 0.5),
@@ -123,7 +123,9 @@ Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotP
         }
       }
 
-    .. code-block:: Python
+   .. code-block:: Python
+
+      # Coming Soon!
 
 
 

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

@@ -17,10 +17,10 @@ const resetTempSettingsStruct = () => {
 const settingsValid = ref(true);
 
 const isValidNetworkTablesIP = (v: string | undefined): boolean => {
-  // Check if it is a valid team number between 1-9999
-  const teamNumberRegex = /^[1-9][0-9]{0,3}$/;
+  // Check if it is a valid team number between 1-99999 (5 digits)
+  const teamNumberRegex = /^[1-9][0-9]{0,4}$/;
   // Check if it is a team number longer than 5 digits
-  const badTeamNumberRegex = /^[0-9]{5,}$/;
+  const badTeamNumberRegex = /^[0-9]{6,}$/;
 
   if (v === undefined) return false;
   if (teamNumberRegex.test(v)) return true;

photon-client/src/types/SettingTypes.ts

@@ -142,7 +142,7 @@ export interface CameraCalibrationResult {
   distCoeffs: JsonMatOfDouble;
   observations: BoardObservation[];
   calobjectWarp?: number[];
-  // We might have to omit observations for bandwith, so backend will send us this
+  // We might have to omit observations for bandwidth, so backend will send us this
   numSnapshots: number;
   meanErrors: number[];
 }

photon-core/build.gradle

@@ -4,6 +4,7 @@ plugins {
 
 import java.nio.file.Path
 
+ext.licenseFile = file("$rootDir/LICENSE")
 apply from: "${rootDir}/shared/common.gradle"
 
 wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
@@ -17,6 +18,7 @@ def nativeTasks = wpilibTools.createExtractionTasks {
 
 nativeTasks.addToSourceSetResources(sourceSets.main)
 
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
@@ -24,7 +26,7 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
-nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())
 
 dependencies {
     // JOGL stuff (currently we only distribute for aarch64, which is Pi 4)

photon-core/settings.gradle

@@ -1 +0,0 @@
-rootProject.name = 'photon-core'

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

@@ -84,15 +84,7 @@ public class CameraConfiguration {
         this.calibrations = new ArrayList<>();
         this.otherPaths = alternates;
 
-        logger.debug(
-                "Creating USB camera configuration for "
-                        + cameraType
-                        + " "
-                        + baseName
-                        + " (AKA "
-                        + nickname
-                        + ") at "
-                        + path);
+        logger.debug("Creating USB camera configuration for " + this.toShortString());
     }
 
     @JsonCreator
@@ -120,15 +112,7 @@ public class CameraConfiguration {
         this.usbPID = usbPID;
         this.usbVID = usbVID;
 
-        logger.debug(
-                "Creating camera configuration for "
-                        + cameraType
-                        + " "
-                        + baseName
-                        + " (AKA "
-                        + nickname
-                        + ") at "
-                        + path);
+        logger.debug("Loaded camera configuration for " + toShortString());
     }
 
     public void addPipelineSettings(List<CVPipelineSettings> settings) {
@@ -189,6 +173,30 @@ public class CameraConfiguration {
         return Arrays.stream(otherPaths).filter(path -> path.contains("/by-path/")).findFirst();
     }
 
+    public String toShortString() {
+        return "CameraConfiguration [baseName="
+                + baseName
+                + ", uniqueName="
+                + uniqueName
+                + ", nickname="
+                + nickname
+                + ", path="
+                + path
+                + ", otherPaths="
+                + Arrays.toString(otherPaths)
+                + ", cameraType="
+                + cameraType
+                + ", cameraQuirks="
+                + cameraQuirks
+                + ", FOV="
+                + FOV
+                + "]"
+                + ", PID="
+                + usbPID
+                + ", VID="
+                + usbVID;
+    }
+
     @Override
     public String toString() {
         return "CameraConfiguration [baseName="

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

@@ -340,7 +340,8 @@ public class ConfigManager {
 
     /**
      * Disable flushing settings to disk as part of our JVM exit hook. Used to prevent uploading all
-     * settings from getting its new configs overwritten at program exit and before theyre all loaded.
+     * settings from getting its new configs overwritten at program exit and before they're all
+     * loaded.
      */
     public void disableFlushOnShutdown() {
         this.flushOnShutdown = false;

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

@@ -265,7 +265,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.HARDWARE_CONFIG), HardwareConfig.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize hardware config! Loading defaults");
+                logger.error("Could not deserialize hardware config! Loading defaults", e);
                 hardwareConfig = new HardwareConfig();
             }
 
@@ -274,7 +274,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.HARDWARE_SETTINGS), HardwareSettings.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize hardware settings! Loading defaults");
+                logger.error("Could not deserialize hardware settings! Loading defaults", e);
                 hardwareSettings = new HardwareSettings();
             }
 
@@ -283,7 +283,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.NETWORK_CONFIG), NetworkConfig.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize network config! Loading defaults");
+                logger.error("Could not deserialize network config! Loading defaults", e);
                 networkConfig = new NetworkConfig();
             }
 
@@ -292,7 +292,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize apriltag layout! Loading defaults");
+                logger.error("Could not deserialize apriltag layout! Loading defaults", e);
                 try {
                     atfl = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
                 } catch (UncheckedIOException e2) {

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -20,7 +20,7 @@ package org.photonvision.common.dataflow.networktables;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
@@ -146,13 +146,19 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             List.of(),
                             result.inputAndOutputFrame);
         else acceptedResult = result;
-        var now = WPIUtilJNI.now();
-        var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos());
+        var now = NetworkTablesJNI.now();
+        var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
+
+        var offset = NetworkTablesManager.getInstance().getOffset();
+
+        // Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's
+        // timebase
         var simplified =
                 new PhotonPipelineResult(
                         acceptedResult.sequenceID,
-                        captureMicros,
-                        now,
+                        captureMicros + offset,
+                        now + offset,
+                        NetworkTablesManager.getInstance().getTimeSinceLastPong(),
                         TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
                         acceptedResult.multiTagResult);
 

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java

@@ -18,6 +18,7 @@
 package org.photonvision.common.dataflow.networktables;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.networktables.LogMessage;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
 import edu.wpi.first.networktables.NetworkTableEvent.Kind;
@@ -26,7 +27,6 @@ import edu.wpi.first.networktables.StringSubscriber;
 import java.io.IOException;
 import java.util.EnumSet;
 import java.util.HashMap;
-import java.util.function.Consumer;
 import org.photonvision.PhotonVersion;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.NetworkConfig;
@@ -34,6 +34,7 @@ import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.scripting.ScriptEventType;
 import org.photonvision.common.scripting.ScriptManager;
@@ -41,32 +42,39 @@ import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.JacksonUtils;
 
 public class NetworkTablesManager {
+    private static final Logger logger =
+            new Logger(NetworkTablesManager.class, LogGroup.NetworkTables);
+
     private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
     private final String kRootTableName = "/photonvision";
     private final String kFieldLayoutName = "apriltag_field_layout";
     public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
 
-    private final NTLogger m_ntLogger = new NTLogger();
-
     private boolean m_isRetryingConnection = false;
 
     private StringSubscriber m_fieldLayoutSubscriber =
             kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
 
+    private final TimeSyncManager m_timeSync = new TimeSyncManager(kRootTable);
+
     private NetworkTablesManager() {
-        ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
-        ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
+        ntInstance.addLogger(
+                LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages
+        ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages
 
         ntInstance.addListener(
                 m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
 
-        TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
-
         // Get the UI state in sync with the backend. NT should fire a callback when it first connects
         // to the robot
         broadcastConnectedStatus();
     }
 
+    public void registerTimedTasks() {
+        m_timeSync.start();
+        TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
+    }
+
     private static NetworkTablesManager INSTANCE;
 
     public static NetworkTablesManager getInstance() {
@@ -74,43 +82,72 @@ public class NetworkTablesManager {
         return INSTANCE;
     }
 
-    private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
+    private void logNtMessage(NetworkTableEvent event) {
+        String levelmsg = "DEBUG";
+        LogLevel pvlevel = LogLevel.DEBUG;
+        if (event.logMessage.level >= LogMessage.kCritical) {
+            pvlevel = LogLevel.ERROR;
+            levelmsg = "CRITICAL";
+        } else if (event.logMessage.level >= LogMessage.kError) {
+            pvlevel = LogLevel.ERROR;
+            levelmsg = "ERROR";
+        } else if (event.logMessage.level >= LogMessage.kWarning) {
+            pvlevel = LogLevel.WARN;
+            levelmsg = "WARNING";
+        } else if (event.logMessage.level >= LogMessage.kInfo) {
+            pvlevel = LogLevel.INFO;
+            levelmsg = "INFO";
+        }
 
-    private static class NTLogger implements Consumer<NetworkTableEvent> {
-        private boolean hasReportedConnectionFailure = false;
+        logger.log(
+                "NT: "
+                        + levelmsg
+                        + " "
+                        + event.logMessage.level
+                        + ": "
+                        + event.logMessage.message
+                        + " ("
+                        + event.logMessage.filename
+                        + ":"
+                        + event.logMessage.line
+                        + ")",
+                pvlevel);
+    }
 
-        @Override
-        public void accept(NetworkTableEvent event) {
-            var isConnEvent = event.is(Kind.kConnected);
-            var isDisconnEvent = event.is(Kind.kDisconnected);
+    public void checkNtConnectState(NetworkTableEvent event) {
+        var isConnEvent = event.is(Kind.kConnected);
+        var isDisconnEvent = event.is(Kind.kDisconnected);
 
-            if (!hasReportedConnectionFailure && isDisconnEvent) {
-                var msg =
-                        String.format(
-                                "NT lost connection to %s:%d! (NT version %d). Will retry in background.",
-                                event.connInfo.remote_ip,
-                                event.connInfo.remote_port,
-                                event.connInfo.protocol_version);
-                logger.error(msg);
-                HardwareManager.getInstance().setNTConnected(false);
+        if (isDisconnEvent) {
+            var msg =
+                    String.format(
+                            "NT lost connection to %s:%d! (NT version %d). Will retry in background.",
+                            event.connInfo.remote_ip,
+                            event.connInfo.remote_port,
+                            event.connInfo.protocol_version);
+            logger.error(msg);
+            HardwareManager.getInstance().setNTConnected(false);
 
-                hasReportedConnectionFailure = true;
-                getInstance().broadcastConnectedStatus();
-            } else if (isConnEvent && event.connInfo != null) {
-                var msg =
-                        String.format(
-                                "NT connected to %s:%d! (NT version %d)",
-                                event.connInfo.remote_ip,
-                                event.connInfo.remote_port,
-                                event.connInfo.protocol_version);
-                logger.info(msg);
-                HardwareManager.getInstance().setNTConnected(true);
+            getInstance().broadcastConnectedStatus();
+        } else if (isConnEvent && event.connInfo != null) {
+            var msg =
+                    String.format(
+                            "NT connected to %s:%d! (NT version %d)",
+                            event.connInfo.remote_ip,
+                            event.connInfo.remote_port,
+                            event.connInfo.protocol_version);
+            logger.info(msg);
+            HardwareManager.getInstance().setNTConnected(true);
 
-                hasReportedConnectionFailure = false;
-                ScriptManager.queueEvent(ScriptEventType.kNTConnected);
-                getInstance().broadcastVersion();
-                getInstance().broadcastConnectedStatus();
-            }
+            ScriptManager.queueEvent(ScriptEventType.kNTConnected);
+            getInstance().broadcastVersion();
+            getInstance().broadcastConnectedStatus();
+
+            m_timeSync.reportNtConnected();
+        } else if (isConnEvent) {
+            logger.warn("Got connection event with no connection info??");
+        } else {
+            logger.warn("Got a non-sensical connection message that is neither connect nor disconnect?");
         }
     }
 
@@ -168,9 +205,16 @@ public class NetworkTablesManager {
         } else {
             setClientMode(config.ntServerAddress);
         }
+
+        m_timeSync.setConfig(config);
+
         broadcastVersion();
     }
 
+    public long getOffset() {
+        return m_timeSync.getOffset();
+    }
+
     private void setClientMode(String ntServerAddress) {
         ntInstance.stopServer();
         ntInstance.startClient4("photonvision");
@@ -211,4 +255,8 @@ public class NetworkTablesManager {
                     "[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
         }
     }
+
+    public long getTimeSinceLastPong() {
+        return m_timeSync.getTimeSinceLastPong();
+    }
 }

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java

@@ -0,0 +1,169 @@
+/*
+ * 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.common.dataflow.networktables;
+
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import org.photonvision.common.configuration.NetworkConfig;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.TimedTaskManager;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncClient;
+import org.photonvision.jni.TimeSyncServer;
+
+public class TimeSyncManager {
+    private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables);
+
+    private TimeSyncClient m_client = null;
+    private TimeSyncServer m_server = null;
+
+    private NetworkTableInstance ntInstance;
+    IntegerPublisher m_offsetPub;
+    IntegerPublisher m_rtt2Pub;
+    IntegerPublisher m_pingsPub;
+    IntegerPublisher m_pongsPub;
+    IntegerPublisher m_lastPongTimePub;
+
+    public TimeSyncManager(NetworkTable kRootTable) {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            logger.error("PhotonTargetingJNI was not loaded! Cannot do time-sync");
+        }
+
+        this.ntInstance = kRootTable.getInstance();
+
+        // Need this subtable to be unique per coprocessor. TODO: consider using MAC address or
+        // something similar for metrics?
+        var timeTable = kRootTable.getSubTable(".timesync").getSubTable(CameraServerJNI.getHostname());
+        m_offsetPub = timeTable.getIntegerTopic("offset_us").publish();
+        m_rtt2Pub = timeTable.getIntegerTopic("rtt2_us").publish();
+        m_pingsPub = timeTable.getIntegerTopic("ping_tx_count").publish();
+        m_pongsPub = timeTable.getIntegerTopic("pong_rx_count").publish();
+        m_lastPongTimePub = timeTable.getIntegerTopic("pong_rx_time_us").publish();
+
+        // default to being a client
+        logger.debug("Starting TimeSyncClient on localhost (for now)");
+        m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
+    }
+
+    // Since we're spinning off tasks in a new thread, be careful and start it seperately
+    public void start() {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            logger.error("PhotonTargetingJNI was not loaded! Cannot start");
+        }
+
+        TimedTaskManager.getInstance().addTask("TimeSyncManager::tick", this::tick, 1000);
+    }
+
+    public synchronized long getOffset() {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            return 0;
+        }
+
+        // if we're a client, return the offset to server time
+        if (m_client != null) return m_client.getOffset();
+        // if we're a server, our time (nt::Now) is the same as network time
+        if (m_server != null) return 0;
+
+        // ????? should never hit
+        logger.error("Client and server and null?");
+        return 0;
+    }
+
+    synchronized void setConfig(NetworkConfig config) {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            return;
+        }
+
+        if (m_client == null && m_server == null) {
+            throw new RuntimeException("Neither client nor server are null?");
+        }
+
+        // if not already running a server, set it up
+        if (config.runNTServer && m_server == null) {
+            // tear down anything old
+            if (m_client != null) {
+                logger.debug("Tearing down old client");
+                m_client.stop();
+                m_client = null;
+            }
+
+            logger.debug("Starting TimeSyncServer");
+            m_server = new TimeSyncServer(5810);
+            m_server.start();
+        } else
+        // if not already running a client, set it up
+        if (m_client == null) {
+            // tear down anything old
+            if (m_server != null) {
+                logger.debug("Tearing down old server");
+                m_server.stop();
+                m_server = null;
+            }
+
+            // Guess at IP -- tick will take care of changing this (may take up to 1 second)
+            logger.debug("Starting TimeSyncClient on localhost (for now)");
+            m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
+        }
+    }
+
+    synchronized void tick() {
+        if (m_client != null) {
+            var conns = ntInstance.getConnections();
+
+            if (conns.length > 0) {
+                logger.debug("Changing TimeSyncClient server to " + conns[0].remote_ip);
+                m_client.setServer(conns[0].remote_ip);
+            }
+
+            if (m_client != null) {
+                var m = m_client.getPingMetadata();
+
+                m_offsetPub.set(m.offset);
+                m_rtt2Pub.set(m.rtt2);
+                m_pingsPub.set(m.pingsSent);
+                m_pongsPub.set(m.pongsReceived);
+                m_lastPongTimePub.set(m.lastPongTime);
+            }
+        }
+    }
+
+    public synchronized long getTimeSinceLastPong() {
+        if (m_client != null) {
+            return m_client.getPingMetadata().timeSinceLastPong();
+        } else if (m_server != null) {
+            return 0;
+        } else {
+            // ????
+            return 0;
+        }
+    }
+
+    /** Restart our timesync client if NT just connected */
+    public synchronized void reportNtConnected() {
+        if (m_client != null) {
+            // restart (in java code; we could just add a reset metrics function...)
+            logger.debug(
+                    "NT (re)connected -- restarting Time Sync Client at " + m_client.getServer() + ":5810");
+            m_client.stop();
+            m_client = new TimeSyncClient(m_client.getServer(), 5810, 1.0);
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioCommand.java

@@ -28,7 +28,7 @@ public enum PigpioCommand {
     PCMD_WVDEL(50), // int wave_delete(unsigned wave_id)
     PCMD_WVTX(51), // int wave_tx_send(unsigned wave_id) (once)
     PCMD_WVTXR(52), // int wave_tx_send(unsigned wave_id) (repeat)
-    PCMD_GDC(83), // int get_duty_cyle(unsigned user_gpio)
+    PCMD_GDC(83), // int get_duty_cycle(unsigned user_gpio)
     PCMD_HP(86), // int hardware_pwm(unsigned gpio, unsigned PWMfreq, unsigned PWMduty)
     PCMD_WVTXM(100); // int wave_tx_send(unsigned wave_id, unsigned wave_mode)
 

photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/RK3588Cmds.java

@@ -27,7 +27,7 @@ public class RK3588Cmds extends LinuxCmds {
         // CPU Temperature
         /* The RK3588 chip has 7 thermal zones that can be accessed via:
          *       /sys/class/thermal/thermal_zoneX/temp
-         * where X is an interger from 0 to 6.
+         * where X is an integer from 0 to 6.
          *
          * || Zone || Location    || Comments                                                ||
          * |    0  |  soc         |  soc thermal (near the center of the chip)                |

photon-core/src/main/java/org/photonvision/common/logging/KernelLogLogger.java

@@ -0,0 +1,77 @@
+/*
+ * 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.common.logging;
+
+import edu.wpi.first.util.RuntimeDetector;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import org.photonvision.common.util.TimedTaskManager;
+import org.photonvision.jni.QueuedFileLogger;
+
+/**
+ * Listens for and reproduces Linux kernel logs, from /var/log/kern.log, into the Photon logger
+ * ecosystem
+ */
+public class KernelLogLogger {
+    private static KernelLogLogger INSTANCE;
+
+    public static KernelLogLogger getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new KernelLogLogger();
+        }
+        return INSTANCE;
+    }
+
+    QueuedFileLogger listener = null;
+    Logger logger = new Logger(KernelLogLogger.class, LogGroup.General);
+
+    public KernelLogLogger() {
+        if (RuntimeDetector.isLinux()) {
+            logger.info("Listening for klogs on /var/log/dmesg ! Boot logs:");
+
+            try {
+                var bootlog = Files.readAllLines(Path.of("/var/log/dmesg"));
+                for (var line : bootlog) {
+                    logger.log(line, LogLevel.DEBUG);
+                }
+            } catch (IOException e) {
+                logger.error("Couldn't read /var/log/dmesg - not printing boot logs");
+            }
+
+            listener = new QueuedFileLogger("/var/log/kern.log");
+        } else {
+            System.out.println("NOT for klogs");
+        }
+
+        // arbitrary frequency to grab logs. The underlying native buffer will grow unbounded without
+        // this, lol
+        TimedTaskManager.getInstance().addTask("outputPrintk", this::outputNewPrintks, 1000);
+    }
+
+    public void outputNewPrintks() {
+        if (listener == null) {
+            return;
+        }
+
+        for (var msg : listener.getNewlines()) {
+            // We currently set all logs to debug regardless of their actual level
+            logger.log(msg, LogLevel.DEBUG);
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java

@@ -25,4 +25,6 @@ public enum LogGroup {
     General,
     Config,
     CSCore,
+    NetworkTables,
+    System,
 }

photon-core/src/main/java/org/photonvision/common/logging/Logger.java

@@ -30,8 +30,34 @@ import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.util.TimedTaskManager;
 
-@SuppressWarnings("unused")
+/** TODO: get rid of static {} blocks and refactor to singleton pattern */
 public class Logger {
+    private static final HashMap<LogGroup, LogLevel> levelMap = new HashMap<>();
+    private static final List<LogAppender> currentAppenders = new ArrayList<>();
+
+    private static final UILogAppender uiLogAppender = new UILogAppender();
+
+    // // TODO why's the logger care about this? split it out
+    // private static KernelLogLogger klogListener = null;
+
+    static {
+        levelMap.put(LogGroup.Camera, LogLevel.INFO);
+        levelMap.put(LogGroup.General, LogLevel.INFO);
+        levelMap.put(LogGroup.WebServer, LogLevel.INFO);
+        levelMap.put(LogGroup.Data, LogLevel.INFO);
+        levelMap.put(LogGroup.VisionModule, LogLevel.INFO);
+        levelMap.put(LogGroup.Config, LogLevel.INFO);
+        levelMap.put(LogGroup.CSCore, LogLevel.TRACE);
+        levelMap.put(LogGroup.NetworkTables, LogLevel.DEBUG);
+        levelMap.put(LogGroup.System, LogLevel.DEBUG);
+
+        currentAppenders.add(new ConsoleLogAppender());
+        currentAppenders.add(uiLogAppender);
+        addFileAppender(PathManager.getInstance().getLogPath());
+
+        cleanLogs(PathManager.getInstance().getLogsDir());
+    }
+
     public static final String ANSI_RESET = "\u001B[0m";
     public static final String ANSI_BLACK = "\u001B[30m";
     public static final String ANSI_RED = "\u001B[31m";
@@ -50,8 +76,6 @@ public class Logger {
     private static final List<Pair<String, LogLevel>> uiBacklog = new ArrayList<>();
     private static boolean connected = false;
 
-    private static final UILogAppender uiLogAppender = new UILogAppender();
-
     private final String className;
     private final LogGroup group;
 
@@ -89,26 +113,6 @@ public class Logger {
         return builder.toString();
     }
 
-    private static final HashMap<LogGroup, LogLevel> levelMap = new HashMap<>();
-    private static final List<LogAppender> currentAppenders = new ArrayList<>();
-
-    static {
-        levelMap.put(LogGroup.Camera, LogLevel.INFO);
-        levelMap.put(LogGroup.General, LogLevel.INFO);
-        levelMap.put(LogGroup.WebServer, LogLevel.INFO);
-        levelMap.put(LogGroup.Data, LogLevel.INFO);
-        levelMap.put(LogGroup.VisionModule, LogLevel.INFO);
-        levelMap.put(LogGroup.Config, LogLevel.INFO);
-        levelMap.put(LogGroup.CSCore, LogLevel.TRACE);
-    }
-
-    static {
-        currentAppenders.add(new ConsoleLogAppender());
-        currentAppenders.add(uiLogAppender);
-        addFileAppender(PathManager.getInstance().getLogPath());
-        cleanLogs(PathManager.getInstance().getLogsDir());
-    }
-
     @SuppressWarnings("ResultOfMethodCallIgnored")
     public static void addFileAppender(Path logFilePath) {
         var file = logFilePath.toFile();
@@ -200,7 +204,7 @@ public class Logger {
         return logLevel.code <= levelMap.get(group).code;
     }
 
-    void log(String message, LogLevel level) {
+    public void log(String message, LogLevel level) {
         if (shouldLog(level)) {
             log(message, level, group, className);
         }

photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java

@@ -19,6 +19,7 @@ package org.photonvision.common.networking;
 
 import java.nio.file.Files;
 import java.nio.file.Path;
+import java.nio.file.Paths;
 import java.util.NoSuchElementException;
 
 import org.photonvision.common.configuration.ConfigManager;
@@ -71,10 +72,7 @@ public class NetworkManager {
         // Start tasks to monitor the network interface(s)
         var ethernetDevices = NetworkUtils.getAllWiredInterfaces();
         for (NMDeviceInfo deviceInfo : ethernetDevices) {
-            var task = "deviceStatus-" + deviceInfo.devName;
-            if (!TimedTaskManager.getInstance().taskActive(task)) {
-                TimedTaskManager.getInstance().addTask(task, deviceStatus(deviceInfo.devName), 5000);
-            }
+            monitorDevice(deviceInfo.devName, 5000);
         }
 
         var physicalDevices = NetworkUtils.getAllActiveWiredInterfaces();
@@ -258,14 +256,22 @@ public class NetworkManager {
     }
 
     // Detects changes in the carrier and reinitializes after re-connect
-    private Runnable deviceStatus(String devName) {
-        Path file = Path.of("/sys/class/net/{device}/carrier".replace("{device}", devName));
-        logger.debug("Watching network interface at path: " + file.toString());
-        var last = new Object() {boolean carrier = true;};
-        return () ->
-        {
+    private void monitorDevice(String devName, int millisInterval) {
+        String taskName = "deviceStatus-" + devName;
+        if (TimedTaskManager.getInstance().taskActive(taskName)) {
+            // task is already running
+            return;
+        }
+        Path path = Paths.get("/sys/class/net/{device}/carrier".replace("{device}", devName));
+        if (Files.notExists(path)) {
+            logger.error("Can't find " + path + ", so can't monitor " + devName);
+            return;
+        }
+        logger.debug("Watching network interface at path: " + path);
+        var last = new Object() {boolean carrier = true; boolean exceptionLogged = false;};
+        Runnable task = () -> {
             try {
-                boolean carrier = Files.readString(file).trim().equals("1");
+                boolean carrier = Files.readString(path).trim().equals("1");
                 if (carrier != last.carrier) {
                     if (carrier) {
                         // carrier came back
@@ -276,9 +282,16 @@ public class NetworkManager {
                     }
                 }
                 last.carrier = carrier;
-            } catch (Exception e) {
-                logger.error("Could not check network status", e);
-            }
-        };
+                last.exceptionLogged = false;
+                } catch (Exception e) {
+                    if (!last.exceptionLogged) {
+                        // Log the exception only once, but keep trying
+                        logger.error("Could not check network status for " + devName, e);
+                        last.exceptionLogged = true;
+                    }
+                }
+            };
+
+        TimedTaskManager.getInstance().addTask(taskName, task, millisInterval);
     }
 }

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

@@ -18,71 +18,20 @@
 package org.photonvision.common.util;
 
 import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.apriltag.jni.AprilTagJNI;
-import edu.wpi.first.cscore.CameraServerCvJNI;
-import edu.wpi.first.cscore.CameraServerJNI;
-import edu.wpi.first.hal.JNIWrapper;
-import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
-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.awt.HeadlessException;
 import java.io.File;
 import java.io.IOException;
 import java.nio.file.Path;
-import org.opencv.core.Core;
 import org.opencv.core.Mat;
 import org.opencv.highgui.HighGui;
+import org.photonvision.jni.WpilibLoader;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
 public class TestUtils {
-    private static boolean has_loaded = false;
-
     public static boolean loadLibraries() {
-        if (has_loaded) return true;
-
-        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
-        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
-        WPIMathJNI.Helper.setExtractOnStaticLoad(false);
-        CameraServerJNI.Helper.setExtractOnStaticLoad(false);
-        CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
-        // OpenCvLoader.Helper.setExtractOnStaticLoad(false);
-        JNIWrapper.Helper.setExtractOnStaticLoad(false);
-        WPINetJNI.Helper.setExtractOnStaticLoad(false);
-        AprilTagJNI.Helper.setExtractOnStaticLoad(false);
-
-        // wpimathjni is a bit odd, it's all in the wpimathjni shared lib, but the java side stuff has
-        // been split.
-        // ArmFeedforwardJNI.Helper.setExtractOnStaticLoad(false);
-        // DAREJNI.Helper.setExtractOnStaticLoad(false);
-        // EigenJNI.Helper.setExtractOnStaticLoad(false);
-        // Ellipse2dJNI.Helper.setExtractOnStaticLoad(false);
-        // Pose3dJNI.Helper.setExtractOnStaticLoad(false);
-        // StateSpaceUtilJNI.Helper.setExtractOnStaticLoad(false);
-        // TrajectoryUtilJNI.Helper.setExtractOnStaticLoad(false);
-
-        try {
-            CombinedRuntimeLoader.loadLibraries(
-                    TestUtils.class,
-                    "wpiutiljni",
-                    "wpimathjni",
-                    "ntcorejni",
-                    "wpinetjni",
-                    "wpiHaljni",
-                    Core.NATIVE_LIBRARY_NAME,
-                    "cscorejni",
-                    "apriltagjni");
-
-            has_loaded = true;
-        } catch (IOException e) {
-            e.printStackTrace();
-            has_loaded = false;
-        }
-
-        return has_loaded;
+        return WpilibLoader.loadLibraries();
     }
 
     @SuppressWarnings("unused")

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

@@ -33,6 +33,7 @@ import java.io.IOException;
 import java.nio.file.Path;
 import java.util.HashMap;
 import java.util.Map;
+import org.eclipse.jetty.io.EofException;
 
 public class JacksonUtils {
     public static class UIMap extends HashMap<String, Object> {}
@@ -76,6 +77,10 @@ public class JacksonUtils {
     }
 
     public static <T> T deserialize(String s, Class<T> ref) throws IOException {
+        if (s.length() == 0) {
+            throw new EofException("Provided empty string for class " + ref.getName());
+        }
+
         PolymorphicTypeValidator ptv =
                 BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
         ObjectMapper objectMapper =

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

@@ -25,7 +25,7 @@ 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 edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.Arrays;
 import java.util.List;
 import org.opencv.core.Core;
@@ -98,7 +98,7 @@ public class MathUtils {
     }
 
     public static long wpiNanoTime() {
-        return microsToNanos(WPIUtilJNI.now());
+        return microsToNanos(NetworkTablesJNI.now());
     }
 
     /**

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

@@ -143,7 +143,7 @@ public class QuirkyCamera {
      * @param usbVid USB VID of camera
      * @param usbPid USB PID of camera
      * @param baseName CSCore name of camera
-     * @param displayName Human-friendly quicky camera name
+     * @param displayName Human-friendly quirky camera name
      * @param quirks Camera quirks
      */
     private QuirkyCamera(
@@ -160,7 +160,7 @@ public class QuirkyCamera {
             this.quirks.put(q, true);
         }
 
-        // (2) for all other quirks in CameraQuirks (in this version of Photon), defalut to false
+        // (2) for all other quirks in CameraQuirks (in this version of Photon), default to false
         for (var q : CameraQuirk.values()) {
             this.quirks.putIfAbsent(q, false);
         }

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

@@ -96,9 +96,12 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
         var autoExpProp = findProperty("exposure_auto", "auto_exposure");
 
         exposureAbsProp = expProp.get();
-        autoExposureProp = autoExpProp.get();
         this.minExposure = exposureAbsProp.getMin();
         this.maxExposure = exposureAbsProp.getMax();
+
+        if (autoExpProp.isPresent()) {
+            autoExposureProp = autoExpProp.get();
+        }
     }
 
     public void setAllCamDefaults() {
@@ -169,7 +172,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
             softSet("auto_exposure_bias", 0);
             softSet("iso_sensitivity_auto", 0); // Disable auto ISO adjustment
             softSet("iso_sensitivity", 0); // Manual ISO adjustment
-            autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
+            if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
             // Most cameras leave exposure time absolute at the last value from their AE
             // algorithm.
@@ -199,7 +202,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
     public void setExposureRaw(double exposureRaw) {
         if (exposureRaw >= 0.0) {
             try {
-                autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
+                if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
                 int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
 
@@ -240,7 +243,8 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
 
     @Override
     public VideoMode getCurrentVideoMode() {
-        return camera.isConnected() ? camera.getVideoMode() : null;
+        return camera
+                .getVideoMode(); // This returns the current video mode even if the camera is disconnected
     }
 
     @Override
@@ -250,7 +254,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
                 logger.error("Got a null video mode! Doing nothing...");
                 return;
             }
-            camera.setVideoMode(videoMode);
+            if (camera.setVideoMode(videoMode)) logger.debug("Failed to set video mode!");
         } catch (Exception e) {
             logger.error("Failed to set video mode!", e);
         }

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

@@ -152,17 +152,17 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
         var matchType = ntMatchType.getAtomic();
         if (matchType.timestamp == 0) {
             // no NT info yet
-            logger.warn("Did not recieve match type, defaulting to 0");
+            logger.warn("Did not receive match type, defaulting to 0");
         }
 
         var matchNum = ntMatchNum.getAtomic();
         if (matchNum.timestamp == 0) {
-            logger.warn("Did not recieve match num, defaulting to -1");
+            logger.warn("Did not receive match num, defaulting to -1");
         }
 
         var eventName = ntEventName.getAtomic();
         if (eventName.timestamp == 0) {
-            logger.warn("Did not recieve event name, defaulting to 'UNKNOWN'");
+            logger.warn("Did not receive event name, defaulting to 'UNKNOWN'");
         }
 
         String matchTypeStr =

photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -42,18 +42,18 @@ public class USBFrameProvider extends CpuImageProcessor {
 
     @Override
     public CapturedFrame getInputMat() {
-        var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread
-        // This is from wpi::Now, or WPIUtilJNI.now()
-        long time =
-                cvSink.grabFrame(mat.getMat())
-                        * 1000; // Units are microseconds, epoch is the same as the Unix epoch
+        // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
+        var mat = new CVMat();
+        // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
+        // Hal::initialize was called
+        long captureTimeNs = cvSink.grabFrame(mat.getMat()) * 1000;
 
-        if (time == 0) {
+        if (captureTimeNs == 0) {
             var error = cvSink.getError();
             logger.error("Error grabbing image: " + error);
         }
 
-        return new CapturedFrame(mat, settables.getFrameStaticProperties(), time);
+        return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java

@@ -59,6 +59,7 @@ public class AprilTagDetectionPipe
     public void setParams(AprilTagDetectionPipeParams newParams) {
         if (this.params == null || !this.params.equals(newParams)) {
             m_detector.setConfig(newParams.detectorParams);
+            m_detector.setQuadThresholdParameters(newParams.quadParams);
 
             m_detector.clearFamilies();
             m_detector.addFamily(newParams.family.getNativeName());

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java

@@ -23,10 +23,15 @@ import org.photonvision.vision.apriltag.AprilTagFamily;
 public class AprilTagDetectionPipeParams {
     public final AprilTagFamily family;
     public final AprilTagDetector.Config detectorParams;
+    public final AprilTagDetector.QuadThresholdParameters quadParams;
 
-    public AprilTagDetectionPipeParams(AprilTagFamily tagFamily, AprilTagDetector.Config config) {
+    public AprilTagDetectionPipeParams(
+            AprilTagFamily tagFamily,
+            AprilTagDetector.Config config,
+            AprilTagDetector.QuadThresholdParameters quadParams) {
         this.family = tagFamily;
         this.detectorParams = config;
+        this.quadParams = quadParams;
     }
 
     @Override
@@ -35,6 +40,7 @@ public class AprilTagDetectionPipeParams {
         int result = 1;
         result = prime * result + ((family == null) ? 0 : family.hashCode());
         result = prime * result + ((detectorParams == null) ? 0 : detectorParams.hashCode());
+        result = prime * result + ((quadParams == null) ? 0 : quadParams.hashCode());
         return result;
     }
 
@@ -46,7 +52,11 @@ public class AprilTagDetectionPipeParams {
         AprilTagDetectionPipeParams other = (AprilTagDetectionPipeParams) obj;
         if (family != other.family) return false;
         if (detectorParams == null) {
-            return other.detectorParams == null;
-        } else return detectorParams.equals(other.detectorParams);
+            if (other.detectorParams != null) return false;
+        } else if (!detectorParams.equals(other.detectorParams)) return false;
+        if (quadParams == null) {
+            if (other.quadParams != null) return false;
+        } else if (!quadParams.equals(other.quadParams)) return false;
+        return true;
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java

@@ -140,7 +140,7 @@ public class CornerDetectionPipe
         var compareDistToTr =
                 Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(2)));
 
-        // top left and top right are the poly corners closest to the bouding box tl and tr
+        // top left and top right are the poly corners closest to the bounding box tl and tr
         pointList.sort(compareDistToTl);
         var tl = pointList.get(0);
         pointList.remove(tl);

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

@@ -87,7 +87,21 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         config.refineEdges = settings.refineEdges;
         config.quadSigma = (float) settings.blur;
         config.quadDecimate = settings.decimate;
-        aprilTagDetectionPipe.setParams(new AprilTagDetectionPipeParams(settings.tagFamily, config));
+
+        var quadParams = new AprilTagDetector.QuadThresholdParameters();
+        // 5 was the default minClusterPixels in WPILib prior to 2025
+        // increasing it causes detection problems when decimate > 1
+        quadParams.minClusterPixels = 5;
+        // these are the same as the values in WPILib 2025
+        // setting them here to prevent upstream changes from changing behavior of the detector
+        quadParams.maxNumMaxima = 10;
+        quadParams.criticalAngle = 45 * Math.PI / 180.0;
+        quadParams.maxLineFitMSE = 10.0f;
+        quadParams.minWhiteBlackDiff = 5;
+        quadParams.deglitch = false;
+
+        aprilTagDetectionPipe.setParams(
+                new AprilTagDetectionPipeParams(settings.tagFamily, config, quadParams));
 
         if (frameStaticProperties.cameraCalibration != null) {
             var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();

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

@@ -512,7 +512,7 @@ public class PipelineManager {
         var oldSettings = userPipelineSettings.get(idx);
 
         var name = getCurrentPipelineSettings().pipelineNickname;
-        // Dummy settings to copy common fileds over
+        // Dummy settings to copy common fields over
         var newSettings = createSettingsForType(type, name);
 
         // Copy all fields from AdvancedPipelineSettings/its superclasses from old to new

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

@@ -30,6 +30,7 @@ import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.hardware.Platform;
+import org.photonvision.common.hardware.Platform.OSType;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;
@@ -128,20 +129,26 @@ public class VisionSourceManager {
         return tryMatchCamImpl(null);
     }
 
+    protected List<VisionSource> tryMatchCamImpl(ArrayList<CameraInfo> cameraInfos) {
+        return tryMatchCamImpl(cameraInfos, Platform.getCurrentPlatform());
+    }
+
     /**
      * @param cameraInfos Used to feed camera info for unit tests.
      * @return New VisionSources.
      */
-    protected List<VisionSource> tryMatchCamImpl(ArrayList<CameraInfo> cameraInfos) {
+    protected List<VisionSource> tryMatchCamImpl(
+            ArrayList<CameraInfo> cameraInfos, Platform platform) {
         boolean createSources = true;
         List<CameraInfo> connectedCameras;
         if (cameraInfos == null) {
             // Detect USB cameras using CSCore
-            connectedCameras = new ArrayList<>(filterAllowedDevices(getConnectedUSBCameras()));
+            connectedCameras = new ArrayList<>(filterAllowedDevices(getConnectedUSBCameras(), platform));
             // Detect CSI cameras using libcamera
-            connectedCameras.addAll(new ArrayList<>(filterAllowedDevices(getConnectedCSICameras())));
+            connectedCameras.addAll(
+                    new ArrayList<>(filterAllowedDevices(getConnectedCSICameras(), platform)));
         } else {
-            connectedCameras = new ArrayList<>(filterAllowedDevices(cameraInfos));
+            connectedCameras = new ArrayList<>(filterAllowedDevices(cameraInfos, platform));
             createSources =
                     false; // Dont create sources if we are using supplied camerainfo for unit tests.
         }
@@ -162,7 +169,7 @@ public class VisionSourceManager {
         // All cameras are already loaded return no new sources.
         if (connectedCameras.isEmpty()) return null;
 
-        logger.debug("Matching " + connectedCameras.size() + " new cameras!");
+        logger.debug("Matching " + connectedCameras.size() + " new camera(s)!");
 
         // Debug prints
         for (var info : connectedCameras) {
@@ -170,7 +177,7 @@ public class VisionSourceManager {
         }
 
         if (!unmatchedLoadedConfigs.isEmpty())
-            logger.debug("Trying to match " + unmatchedLoadedConfigs.size() + " unmatched configs...");
+            logger.debug("Trying to match " + unmatchedLoadedConfigs.size() + " unmatched config(s)...");
 
         // Match camera configs to physical cameras
         List<CameraConfiguration> matchedCameras =
@@ -182,7 +189,7 @@ public class VisionSourceManager {
                     () ->
                             "After matching, "
                                     + unmatchedLoadedConfigs.size()
-                                    + " configs remained unmatched. Is your camera disconnected?");
+                                    + " config(s) remained unmatched. Is your camera disconnected?");
             logger.warn(
                     "Unloaded configs: "
                             + unmatchedLoadedConfigs.stream()
@@ -233,7 +240,7 @@ public class VisionSourceManager {
         if (checkUSBPath && savedConfig.getUSBPath().isEmpty()) {
             logger.debug(
                     "WARN: Camera has empty USB path, but asked to match by name: "
-                            + camCfgToString(savedConfig));
+                            + savedConfig.toShortString());
         }
 
         return (CameraInfo physicalCamera) -> {
@@ -277,22 +284,6 @@ public class VisionSourceManager {
                 ConfigManager.getInstance().getConfig().getNetworkConfig().matchCamerasOnlyByPath);
     }
 
-    private static final String camCfgToString(CameraConfiguration c) {
-        return new StringBuilder()
-                .append("[baseName=")
-                .append(c.baseName)
-                .append(", uniqueName=")
-                .append(c.uniqueName)
-                .append(", otherPaths=")
-                .append(Arrays.toString(c.otherPaths))
-                .append(", vid=")
-                .append(c.usbVID)
-                .append(", pid=")
-                .append(c.usbPID)
-                .append("]")
-                .toString();
-    }
-
     /**
      * Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on
      * disk.
@@ -423,7 +414,7 @@ public class VisionSourceManager {
                 logger.debug(
                         String.format(
                                 "Trying to find a match for loaded camera %s (%s) with camera config: %s",
-                                config.baseName, config.uniqueName, camCfgToString(config)));
+                                config.baseName, config.uniqueName, config.toShortString()));
 
                 // Get matcher and filter against it, picking out the first match
                 Predicate<CameraInfo> matches =
@@ -463,7 +454,7 @@ public class VisionSourceManager {
             List<CameraConfiguration> loadedConfigs) {
         List<CameraConfiguration> ret = new ArrayList<CameraConfiguration>();
         logger.debug(
-                "After matching loaded configs, these configs remained unmatched: "
+                "After matching loaded configs, these cameras remained unmatched: "
                         + detectedCameraList.stream()
                                 .map(n -> String.valueOf(n))
                                 .collect(Collectors.joining("-", "{", "}")));
@@ -535,9 +526,9 @@ public class VisionSourceManager {
      * Filter out any blacklisted or ignored devices.
      *
      * @param allDevices
-     * @return list of devices with blacklisted or ingore devices removed.
+     * @return list of devices with blacklisted or ignore devices removed.
      */
-    private List<CameraInfo> filterAllowedDevices(List<CameraInfo> allDevices) {
+    private List<CameraInfo> filterAllowedDevices(List<CameraInfo> allDevices, Platform platform) {
         List<CameraInfo> filteredDevices = new ArrayList<>();
         for (var device : allDevices) {
             if (deviceBlacklist.contains(device.name)) {
@@ -546,6 +537,13 @@ public class VisionSourceManager {
             } else if (device.name.matches(ignoredCamerasRegex)) {
                 logger.trace("Skipping ignored device: \"" + device.name + "\" at \"" + device.path);
             } else if (device.getIsV4lCsiCamera()) {
+            } else if (device.otherPaths.length == 0
+                    && platform.osType == OSType.LINUX
+                    && device.cameraType == CameraType.UsbCamera) {
+                logger.trace(
+                        "Skipping device with no other paths: \"" + device.name + "\" at \"" + device.path);
+                // If cscore hasnt passed this other paths aka a path by id or a path as in usb port then we
+                // cant guarantee it is a valid camera.
             } else {
                 filteredDevices.add(device);
                 logger.trace(
@@ -559,8 +557,6 @@ public class VisionSourceManager {
             List<CameraConfiguration> camConfigs, boolean createSources) {
         var cameraSources = new ArrayList<VisionSource>();
         for (var configuration : camConfigs) {
-            logger.debug("Creating VisionSource for " + camCfgToString(configuration));
-
             // In unit tests, create dummy
             if (!createSources) {
                 cameraSources.add(new TestSource(configuration));
@@ -580,6 +576,7 @@ public class VisionSourceManager {
                     cameraSources.add(newCam);
                 }
             }
+            logger.debug("Creating VisionSource for " + configuration.toShortString());
         }
         return cameraSources;
     }

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

@@ -59,7 +59,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
-    // Overriden by cameras with AWB gain support
+    // Overriddenn by cameras with AWB gain support
     public void setRedGain(int red) {}
 
     public void setBlueGain(int blue) {}

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

@@ -287,7 +287,7 @@ public class Calibrate3dPipeTest {
     }
 
     /**
-     * Uses a given camera coefficents matrix set to "undistort" every image file found in a given
+     * Uses a given camera coefficientss 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 origional points
+        // Distort the origonal points
         var distortedOriginalPoints =
                 OpenCVHelp.distortPoints(
                         List.of(originalPoints),
@@ -153,14 +153,14 @@ public class CalibrationRotationPipeTest {
                         .collect(Collectors.toList());
 
         // Now let's instead rotate then distort
-        var rotatedOrigionalPoints =
+        var rotatedOriginalPoints =
                 Arrays.stream(originalPoints)
                         .map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight))
                         .collect(Collectors.toList());
 
         var distortedRotatedPoints =
                 OpenCVHelp.distortPoints(
-                        rotatedOrigionalPoints,
+                        rotatedOriginalPoints,
                         rotatedFrameProps.cameraCalibration.getCameraIntrinsicsMat(),
                         rotatedFrameProps.cameraCalibration.getDistCoeffsMat());
 

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -18,8 +18,10 @@
 package org.photonvision.vision.processes;
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
 
 import edu.wpi.first.cscore.VideoMode;
+import java.io.IOException;
 import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
@@ -31,6 +33,7 @@ import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.CVPipelineResultConsumer;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.camera.USBCameras.USBCameraSource;
 import org.photonvision.vision.frame.FrameProvider;
@@ -41,7 +44,16 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
 public class VisionModuleManagerTest {
     @BeforeAll
     public static void init() {
+        String classpathStr = System.getProperty("java.class.path");
+        System.out.print(classpathStr);
+
         TestUtils.loadLibraries();
+        try {
+            if (!PhotonTargetingJniLoader.load()) fail();
+        } catch (UnsatisfiedLinkError | IOException e) {
+            e.printStackTrace();
+            fail(e);
+        }
     }
 
     private static class TestSource extends VisionSource {

photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java

@@ -17,14 +17,14 @@
 
 package org.photonvision.vision.processes;
 
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.*;
 
 import java.util.ArrayList;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
@@ -62,7 +62,8 @@ public class VisionSourceManagerTest {
         config4.usbVID = 5;
         config4.usbPID = 6;
 
-        CameraInfo info1 = new CameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2);
+        CameraInfo info1 =
+                new CameraInfo(0, "dev/video0", "testVideo", new String[] {"/usb/path/0"}, 1, 2);
 
         cameraInfos.add(info1);
 
@@ -73,7 +74,8 @@ public class VisionSourceManagerTest {
         assertTrue(inst.knownCameras.contains(info1));
         assertEquals(2, inst.unmatchedLoadedConfigs.size());
 
-        CameraInfo info2 = new CameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 3);
+        CameraInfo info2 =
+                new CameraInfo(0, "dev/video1", "secondTestVideo", new String[] {"/usb/path/1"}, 2, 3);
 
         cameraInfos.add(info2);
 
@@ -297,7 +299,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video0",
                         CAM1_OLD_PATHS);
         camera1_saved_config.usbVID = 3141;
@@ -306,7 +308,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera (1)",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video2",
                         CAM2_OLD_PATH);
         camera2_saved_config.usbVID = 3141;
@@ -362,7 +364,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera (1)",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video0",
                         CAM1_OLD_PATHS);
         camera1_saved_config.usbVID = 3141;
@@ -371,7 +373,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera (1)",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video2",
                         CAM2_OLD_PATH);
         camera2_saved_config.usbVID = 3141;
@@ -500,6 +502,43 @@ public class VisionSourceManagerTest {
         }
     }
 
+    @Test
+    public void testNoOtherPaths() {
+        Logger.setLevel(LogGroup.Camera, LogLevel.DEBUG);
+
+        // List of known cameras
+        var cameraInfos = new ArrayList<CameraInfo>();
+
+        var inst = new VisionSourceManager();
+        ConfigManager.getInstance().clearConfig();
+        ConfigManager.getInstance().load();
+        ConfigManager.getInstance().getConfig().getNetworkConfig().matchCamerasOnlyByPath = false;
+
+        // Match empty camera infos
+        inst.tryMatchCamImpl(cameraInfos);
+
+        CameraInfo info1 =
+                new CameraInfo(0, "/dev/video0", "Arducam OV2311 USB Camera", new String[] {}, 3141, 25446);
+
+        cameraInfos.add(info1);
+
+        // Match two "new" cameras
+        var ret1 = inst.tryMatchCamImpl(cameraInfos, Platform.LINUX_64);
+
+        // Our cameras should be "known"
+        assertFalse(inst.knownCameras.contains(info1));
+        assertEquals(0, inst.knownCameras.size());
+        assertEquals(null, ret1);
+
+        // Match two "new" cameras
+        var ret2 = inst.tryMatchCamImpl(cameraInfos, Platform.WINDOWS_64);
+
+        // Our cameras should be "known"
+        assertTrue(inst.knownCameras.contains(info1));
+        assertEquals(1, inst.knownCameras.size());
+        assertEquals(1, ret2.size());
+    }
+
     @Test
     public void testIdenticalCameras() {
         Logger.setLevel(LogGroup.Camera, LogLevel.DEBUG);
@@ -571,7 +610,7 @@ public class VisionSourceManagerTest {
                     ret1.stream().filter(it -> thisName.equals(it.cameraConfiguration.uniqueName)).count());
         }
 
-        // duplciate cameras, same info, new ref
+        // duplicate cameras, same info, new ref
         var duplicateCameraInfos = new ArrayList<CameraInfo>();
         CameraInfo info1_dup =
                 new CameraInfo(
@@ -609,7 +648,7 @@ public class VisionSourceManagerTest {
         assertTrue(inst.knownCameras.contains(info2_dup));
         assertEquals(2, inst.knownCameras.size());
 
-        // duplciate cameras this simulates unplugging one and plugging the other in where v4l assigns
+        // duplicate cameras this simulates unplugging one and plugging the other in where v4l assigns
         // the same by-id path to the other camera
         var duplicateCameraInfos1 = new ArrayList<CameraInfo>();
         CameraInfo info3_dup =

photon-docs/build.gradle

@@ -217,7 +217,7 @@ task generateJavaDocs(type: Javadoc) {
     classpath = files(exportedProjects.collect { project(it).sourceSets.main.compileClasspath })
     dependsOn project(':photon-core').writeCurrentVersion
 
-    options.links("https://docs.oracle.com/en/java/javase/17/docs/api/")
+    options.links "https://docs.oracle.com/en/java/javase/17/docs/api/", "https://github.wpilib.org/allwpilib/docs/release/java/"
     options.addStringOption("tag", "pre:a:Pre-Condition")
     options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true)
     options.addBooleanOption('html5', true)

photon-lib/build.gradle

@@ -9,6 +9,7 @@ ext {
     includePhotonTargeting = true
     // Include the generated Version file
     generatedHeaders = "src/generate/native/include"
+    licenseFile = file("LICENSE")
 }
 
 apply plugin: 'cpp'
@@ -177,6 +178,7 @@ task generateVendorJson() {
     def read = photonlibFileInput.text
             .replace('${photon_version}', pubVersion)
             .replace('${frc_year}', frcYear)
+            .replace('${wpilib_version}', wpilibVersion)
     photonlibFileOutput.text = read
 
     outputs.upToDateWhen { false }
@@ -331,6 +333,7 @@ def nativeTasks = wpilibTools.createExtractionTasks {
 
 nativeTasks.addToSourceSetResources(sourceSets.test)
 
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
@@ -338,4 +341,4 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
-nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())

photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py

@@ -21,6 +21,7 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 
 class MultiTargetPNPResultSerde:
@@ -28,6 +29,17 @@ class MultiTargetPNPResultSerde:
     MESSAGE_VERSION = "541096947e9f3ca2d3f425ff7b04aa7b"
     MESSAGE_FORMAT = "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];"
 
+    @staticmethod
+    def pack(value: "MultiTargetPNPResult") -> "Packet":
+        ret = Packet()
+
+        # estimatedPose is of non-intrinsic type PnpResult
+        ret.encodeBytes(PnpResult.photonStruct.pack(value.estimatedPose).getData())
+
+        # fiducialIDsUsed is a custom VLA!
+        ret.encodeShortList(value.fiducialIDsUsed)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "MultiTargetPNPResult":
         ret = MultiTargetPNPResult()

photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py

@@ -21,14 +21,30 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 
 class PhotonPipelineMetadataSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "626e70461cbdb274fb43ead09c255f4e"
-    MESSAGE_FORMAT = (
-        "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"
-    )
+    MESSAGE_VERSION = "ac0a45f686457856fb30af77699ea356"
+    MESSAGE_FORMAT = "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;int64 timeSinceLastPong;"
+
+    @staticmethod
+    def pack(value: "PhotonPipelineMetadata") -> "Packet":
+        ret = Packet()
+
+        # sequenceID is of intrinsic type int64
+        ret.encodeLong(value.sequenceID)
+
+        # captureTimestampMicros is of intrinsic type int64
+        ret.encodeLong(value.captureTimestampMicros)
+
+        # publishTimestampMicros is of intrinsic type int64
+        ret.encodeLong(value.publishTimestampMicros)
+
+        # timeSinceLastPong is of intrinsic type int64
+        ret.encodeLong(value.timeSinceLastPong)
+        return ret
 
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonPipelineMetadata":
@@ -43,6 +59,9 @@ class PhotonPipelineMetadataSerde:
         # publishTimestampMicros is of intrinsic type int64
         ret.publishTimestampMicros = packet.decodeLong()
 
+        # timeSinceLastPong is of intrinsic type int64
+        ret.timeSinceLastPong = packet.decodeLong()
+
         return ret
 
 

photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py

@@ -21,12 +21,29 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 
 class PhotonPipelineResultSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "5eeaa293d0c69aea90eaddea786a2b3b"
-    MESSAGE_FORMAT = "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
+    MESSAGE_VERSION = "4b2ff16a964b5e2bf04be0c1454d91c4"
+    MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
+
+    @staticmethod
+    def pack(value: "PhotonPipelineResult") -> "Packet":
+        ret = Packet()
+
+        # metadata is of non-intrinsic type PhotonPipelineMetadata
+        ret.encodeBytes(
+            PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()
+        )
+
+        # targets is a custom VLA!
+        ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
+
+        # multitagResult is optional! it better not be a VLA too
+        ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct)
+        return ret
 
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonPipelineResult":

photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py

@@ -21,6 +21,7 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 
 class PhotonTrackedTargetSerde:
@@ -28,6 +29,45 @@ class PhotonTrackedTargetSerde:
     MESSAGE_VERSION = "cc6dbb5c5c1e0fa808108019b20863f1"
     MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 minAreaRectCorners[?];TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 detectedCorners[?];"
 
+    @staticmethod
+    def pack(value: "PhotonTrackedTarget") -> "Packet":
+        ret = Packet()
+
+        # yaw is of intrinsic type float64
+        ret.encodeDouble(value.yaw)
+
+        # pitch is of intrinsic type float64
+        ret.encodeDouble(value.pitch)
+
+        # area is of intrinsic type float64
+        ret.encodeDouble(value.area)
+
+        # skew is of intrinsic type float64
+        ret.encodeDouble(value.skew)
+
+        # fiducialId is of intrinsic type int32
+        ret.encodeInt(value.fiducialId)
+
+        # objDetectId is of intrinsic type int32
+        ret.encodeInt(value.objDetectId)
+
+        # objDetectConf is of intrinsic type float32
+        ret.encodeFloat(value.objDetectConf)
+
+        ret.encodeTransform(value.bestCameraToTarget)
+
+        ret.encodeTransform(value.altCameraToTarget)
+
+        # poseAmbiguity is of intrinsic type float64
+        ret.encodeDouble(value.poseAmbiguity)
+
+        # minAreaRectCorners is a custom VLA!
+        ret.encodeList(value.minAreaRectCorners, TargetCorner.photonStruct)
+
+        # detectedCorners is a custom VLA!
+        ret.encodeList(value.detectedCorners, TargetCorner.photonStruct)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonTrackedTarget":
         ret = PhotonTrackedTarget()

photon-lib/py/photonlibpy/generated/PnpResultSerde.py

@@ -21,6 +21,7 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 
 class PnpResultSerde:
@@ -28,6 +29,24 @@ class PnpResultSerde:
     MESSAGE_VERSION = "ae4d655c0a3104d88df4f5db144c1e86"
     MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
 
+    @staticmethod
+    def pack(value: "PnpResult") -> "Packet":
+        ret = Packet()
+
+        ret.encodeTransform(value.best)
+
+        ret.encodeTransform(value.alt)
+
+        # bestReprojErr is of intrinsic type float64
+        ret.encodeDouble(value.bestReprojErr)
+
+        # altReprojErr is of intrinsic type float64
+        ret.encodeDouble(value.altReprojErr)
+
+        # ambiguity is of intrinsic type float64
+        ret.encodeDouble(value.ambiguity)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PnpResult":
         ret = PnpResult()

photon-lib/py/photonlibpy/generated/TargetCornerSerde.py

@@ -21,6 +21,7 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 
 class TargetCornerSerde:
@@ -28,6 +29,17 @@ class TargetCornerSerde:
     MESSAGE_VERSION = "16f6ac0dedc8eaccb951f4895d9e18b6"
     MESSAGE_FORMAT = "float64 x;float64 y;"
 
+    @staticmethod
+    def pack(value: "TargetCorner") -> "Packet":
+        ret = Packet()
+
+        # x is of intrinsic type float64
+        ret.encodeDouble(value.x)
+
+        # y is of intrinsic type float64
+        ret.encodeDouble(value.y)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "TargetCorner":
         ret = TargetCorner()

photon-lib/py/photonlibpy/packet.py

@@ -22,7 +22,7 @@ import wpilib
 
 
 class Packet:
-    def __init__(self, data: bytes):
+    def __init__(self, data: bytes = b""):
         """
         * Constructs an empty packet.
         *
@@ -198,3 +198,110 @@ class Packet:
             return serde.unpack(self)
         else:
             return None
+
+    def _encodeGeneric(self, packFormat, value):
+        """
+        Append bytes to the packet data buffer.
+        """
+        self.packetData = self.packetData + struct.pack(packFormat, value)
+        self.size = len(self.packetData)
+
+    def encode8(self, value: int):
+        """
+        Encodes a single byte and appends it to the packet.
+        """
+        self._encodeGeneric("<b", value)
+
+    def encode16(self, value: int):
+        """
+        Encodes a short (2 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<h", value)
+
+    def encodeInt(self, value: int):
+        """
+        Encodes an int (4 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<l", value)
+
+    def encodeFloat(self, value: float):
+        """
+        Encodes a float (4 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<f", value)
+
+    def encodeLong(self, value: int):
+        """
+        Encodes a long (8 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<q", value)
+
+    def encodeDouble(self, value: float):
+        """
+        Encodes a double (8 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<d", value)
+
+    def encodeBoolean(self, value: bool):
+        """
+        Encodes a boolean as a single byte and appends it to the packet.
+        """
+        self.encode8(1 if value else 0)
+
+    def encodeDoubleArray(self, values: list[float]):
+        """
+        Encodes an array of doubles and appends it to the packet.
+        """
+        self.encode8(len(values))
+        for value in values:
+            self.encodeDouble(value)
+
+    def encodeShortList(self, values: list[int]):
+        """
+        Encodes a list of shorts, with length prefixed as a single byte.
+        """
+        self.encode8(len(values))
+        for value in values:
+            self.encode16(value)
+
+    def encodeTransform(self, transform: Transform3d):
+        """
+        Encodes a Transform3d (translation and rotation) and appends it to the packet.
+        """
+        # Encode Translation3d part (x, y, z)
+        self.encodeDouble(transform.translation().x)
+        self.encodeDouble(transform.translation().y)
+        self.encodeDouble(transform.translation().z)
+
+        # Encode Rotation3d as Quaternion (w, x, y, z)
+        quaternion = transform.rotation().getQuaternion()
+        self.encodeDouble(quaternion.W())
+        self.encodeDouble(quaternion.X())
+        self.encodeDouble(quaternion.Y())
+        self.encodeDouble(quaternion.Z())
+
+    def encodeList(self, values: list[Any], serde: Type):
+        """
+        Encodes a list of items using a specific serializer and appends it to the packet.
+        """
+        self.encode8(len(values))
+        for item in values:
+            packed = serde.pack(item)
+            self.packetData = self.packetData + packed.getData()
+            self.size = len(self.packetData)
+
+    def encodeOptional(self, value: Optional[Any], serde: Type):
+        """
+        Encodes an optional value using a specific serializer.
+        """
+        if value is None:
+            self.encodeBoolean(False)
+        else:
+            self.encodeBoolean(True)
+            packed = serde.pack(value)
+            self.packetData = self.packetData + packed.getData()
+            self.size = len(self.packetData)
+
+    def encodeBytes(self, value: bytes):
+        self.packetData = self.packetData + value
+        self.size = len(self.packetData)

photon-lib/py/photonlibpy/photonCamera.py

@@ -124,7 +124,7 @@ class PhotonCamera:
                 pkt = Packet(byteList)
                 newResult = PhotonPipelineResult.photonStruct.unpack(pkt)
                 # NT4 allows us to correct the timestamp based on when the message was sent
-                newResult.ntReceiveTimestampMicros = timestamp / 1e6
+                newResult.ntReceiveTimestampMicros = timestamp
                 ret.append(newResult)
 
         return ret

photon-lib/py/photonlibpy/photonPoseEstimator.py

@@ -269,8 +269,8 @@ class PhotonPoseEstimator:
     def _multiTagOnCoprocStrategy(
         self, result: PhotonPipelineResult
     ) -> Optional[EstimatedRobotPose]:
-        if result.multiTagResult.estimatedPose.isPresent:
-            best_tf = result.multiTagResult.estimatedPose.best
+        if result.multitagResult is not None:
+            best_tf = result.multitagResult.estimatedPose.best
             best = (
                 Pose3d()
                 .transformBy(best_tf)  # field-to-camera

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

@@ -8,8 +8,8 @@ class PnpResult:
     best: Transform3d = field(default_factory=Transform3d)
     alt: Transform3d = field(default_factory=Transform3d)
     ambiguity: float = 0.0
-    bestReprojError: float = 0.0
-    altReprojError: float = 0.0
+    bestReprojErr: float = 0.0
+    altReprojErr: float = 0.0
 
     photonStruct: "PNPResultSerde" = None
 

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

@@ -15,6 +15,8 @@ class PhotonPipelineMetadata:
     # Mirror of the heartbeat entry -- monotonically increasing
     sequenceID: int = -1
 
+    timeSinceLastPong: int = -1
+
     photonStruct: "PhotonPipelineMetadataSerde" = None
 
 
@@ -24,8 +26,10 @@ class PhotonPipelineResult:
     ntReceiveTimestampMicros: int = -1
 
     targets: list[PhotonTrackedTarget] = field(default_factory=list)
+    # Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
+    # an arbitrary timebase. This is not true in C++ or Java.
     metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
-    multiTagResult: Optional[MultiTargetPNPResult] = None
+    multitagResult: Optional[MultiTargetPNPResult] = None
 
     def getLatencyMillis(self) -> float:
         return (

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

@@ -13,9 +13,11 @@ class PhotonTrackedTarget:
     fiducialId: int = -1
     bestCameraToTarget: Transform3d = field(default_factory=Transform3d)
     altCameraToTarget: Transform3d = field(default_factory=Transform3d)
-    minAreaRectCorners: list[TargetCorner] | None = None
-    detectedCorners: list[TargetCorner] | None = None
+    minAreaRectCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
+    detectedCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
     poseAmbiguity: float = 0.0
+    objDetectId: int = -1
+    objDetectConf: float = 0.0
 
     def getYaw(self) -> float:
         return self.yaw
@@ -35,10 +37,10 @@ class PhotonTrackedTarget:
     def getPoseAmbiguity(self) -> float:
         return self.poseAmbiguity
 
-    def getMinAreaRectCorners(self) -> list[TargetCorner] | None:
+    def getMinAreaRectCorners(self) -> list[TargetCorner]:
         return self.minAreaRectCorners
 
-    def getDetectedCorners(self) -> list[TargetCorner] | None:
+    def getDetectedCorners(self) -> list[TargetCorner]:
         return self.detectedCorners
 
     def getBestCameraToTarget(self) -> Transform3d:

photon-lib/py/test/photonPoseEstimator_test.py

@@ -15,247 +15,260 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-# from photonlibpy import MultiTargetPNPResult, PnpResult
-# from photonlibpy import PhotonPipelineResult
-# from photonlibpy import PhotonPoseEstimator, PoseStrategy
-# from photonlibpy import PhotonTrackedTarget, TargetCorner, PhotonPipelineMetadata
-# from robotpy_apriltag import AprilTag, AprilTagFieldLayout
-# from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
+from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult
+from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult
+from photonlibpy import PhotonPoseEstimator, PoseStrategy
+from photonlibpy.targeting import (
+    PhotonTrackedTarget,
+    TargetCorner,
+    PhotonPipelineMetadata,
+)
+from robotpy_apriltag import AprilTag, AprilTagFieldLayout
+from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
 
 
-# class PhotonCameraInjector:
-#     result: PhotonPipelineResult
+class PhotonCameraInjector:
+    result: PhotonPipelineResult
 
-#     def getLatestResult(self) -> PhotonPipelineResult:
-#         return self.result
+    def getLatestResult(self) -> PhotonPipelineResult:
+        return self.result
 
 
-# def setupCommon() -> AprilTagFieldLayout:
-#     tagList = []
-#     tagPoses = (
-#         Pose3d(3, 3, 3, Rotation3d()),
-#         Pose3d(5, 5, 5, Rotation3d()),
-#     )
-#     for id_, pose in enumerate(tagPoses):
-#         aprilTag = AprilTag()
-#         aprilTag.ID = id_
-#         aprilTag.pose = pose
-#         tagList.append(aprilTag)
+def setupCommon() -> AprilTagFieldLayout:
+    tagList = []
+    tagPoses = (
+        Pose3d(3, 3, 3, Rotation3d()),
+        Pose3d(5, 5, 5, Rotation3d()),
+    )
+    for id_, pose in enumerate(tagPoses):
+        aprilTag = AprilTag()
+        aprilTag.ID = id_
+        aprilTag.pose = pose
+        tagList.append(aprilTag)
 
-#     fieldLength = 54 / 3.281  # 54 ft -> meters
-#     fieldWidth = 27 / 3.281  # 24 ft -> meters
+    fieldLength = 54 / 3.281  # 54 ft -> meters
+    fieldWidth = 27 / 3.281  # 24 ft -> meters
 
-#     return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
+    return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
 
 
-# def test_lowestAmbiguityStrategy():
-#     aprilTags = setupCommon()
+def test_lowestAmbiguityStrategy():
+    aprilTags = setupCommon()
 
-#     cameraOne = PhotonCameraInjector()
-#     cameraOne.result = PhotonPipelineResult(
-#         11 * 1e6,
-#         [
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.0,
-#                 4.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.7,
-#             ),
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.1,
-#                 6.7,
-#                 1,
-#                 Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
-#                 Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.3,
-#             ),
-#             PhotonTrackedTarget(
-#                 9.0,
-#                 -2.0,
-#                 19.0,
-#                 3.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.4,
-#             ),
-#         ],
-#         None,
-#         metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
-#     )
+    cameraOne = PhotonCameraInjector()
+    cameraOne.result = PhotonPipelineResult(
+        int(11 * 1e6),
+        [
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.0,
+                4.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.7,
+            ),
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.1,
+                6.7,
+                1,
+                Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
+                Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.3,
+            ),
+            PhotonTrackedTarget(
+                9.0,
+                -2.0,
+                19.0,
+                3.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.4,
+            ),
+        ],
+        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+        multitagResult=None,
+    )
 
-#     estimator = PhotonPoseEstimator(
-#         aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
-#     )
+    estimator = PhotonPoseEstimator(
+        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+    )
 
-#     estimatedPose = estimator.update()
-#     pose = estimatedPose.estimatedPose
+    estimatedPose = estimator.update()
 
-#     assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
-#     assertEquals(1, pose.x, 0.01)
-#     assertEquals(3, pose.y, 0.01)
-#     assertEquals(2, pose.z, 0.01)
+    assert estimatedPose is not None
+
+    pose = estimatedPose.estimatedPose
+
+    assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
+    assertEquals(1, pose.x, 0.01)
+    assertEquals(3, pose.y, 0.01)
+    assertEquals(2, pose.z, 0.01)
 
 
-# def test_multiTagOnCoprocStrategy():
-#     cameraOne = PhotonCameraInjector()
-#     cameraOne.result = PhotonPipelineResult(
-#         11 * 1e6,
-#         # There needs to be at least one target present for pose estimation to work
-#         # Doesn't matter which/how many targets for this test
-#         [
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.0,
-#                 4.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.7,
-#             )
-#         ],
-#         multiTagResult=MultiTargetPNPResult(
-#             PnpResult(True, Transform3d(1, 3, 2, Rotation3d()))
-#         ),
-#         metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
-#     )
+def test_multiTagOnCoprocStrategy():
+    cameraOne = PhotonCameraInjector()
+    cameraOne.result = PhotonPipelineResult(
+        int(11 * 1e6),
+        # There needs to be at least one target present for pose estimation to work
+        # Doesn't matter which/how many targets for this test
+        [
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.0,
+                4.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.7,
+            )
+        ],
+        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+        multitagResult=MultiTargetPNPResult(
+            PnpResult(Transform3d(1, 3, 2, Rotation3d()))
+        ),
+    )
 
-#     estimator = PhotonPoseEstimator(
-#         AprilTagFieldLayout(),
-#         PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-#         cameraOne,
-#         Transform3d(),
-#     )
+    estimator = PhotonPoseEstimator(
+        AprilTagFieldLayout(),
+        PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+        cameraOne,
+        Transform3d(),
+    )
 
-#     estimatedPose = estimator.update()
-#     pose = estimatedPose.estimatedPose
+    estimatedPose = estimator.update()
 
-#     assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
-#     assertEquals(1, pose.x, 0.01)
-#     assertEquals(3, pose.y, 0.01)
-#     assertEquals(2, pose.z, 0.01)
+    assert estimatedPose is not None
+
+    pose = estimatedPose.estimatedPose
+
+    assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
+    assertEquals(1, pose.x, 0.01)
+    assertEquals(3, pose.y, 0.01)
+    assertEquals(2, pose.z, 0.01)
 
 
-# def test_cacheIsInvalidated():
-#     aprilTags = setupCommon()
+def test_cacheIsInvalidated():
+    aprilTags = setupCommon()
 
-#     cameraOne = PhotonCameraInjector()
-#     result = PhotonPipelineResult(
-#         20 * 1e6,
-#         [
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.0,
-#                 4.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.7,
-#             )
-#         ],
-#         metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
-#     )
+    cameraOne = PhotonCameraInjector()
+    result = PhotonPipelineResult(
+        int(20 * 1e6),
+        [
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.0,
+                4.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.7,
+            )
+        ],
+        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+    )
 
-#     estimator = PhotonPoseEstimator(
-#         aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
-#     )
+    estimator = PhotonPoseEstimator(
+        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+    )
 
-#     # Empty result, expect empty result
-#     cameraOne.result = PhotonPipelineResult(0)
-#     estimatedPose = estimator.update()
-#     assert estimatedPose is None
+    # Empty result, expect empty result
+    cameraOne.result = PhotonPipelineResult(0)
+    estimatedPose = estimator.update()
+    assert estimatedPose is None
 
-#     # Set actual result
-#     cameraOne.result = result
-#     estimatedPose = estimator.update()
-#     assert estimatedPose is not None
-#     assertEquals(20, estimatedPose.timestampSeconds, 0.01)
-#     assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    # Set actual result
+    cameraOne.result = result
+    estimatedPose = estimator.update()
+    assert estimatedPose is not None
+    assertEquals(20, estimatedPose.timestampSeconds, 0.01)
+    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
 
-#     # And again -- pose cache should mean this is empty
-#     cameraOne.result = result
-#     estimatedPose = estimator.update()
-#     assert estimatedPose is None
-#     # Expect the old timestamp to still be here
-#     assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    # And again -- pose cache should mean this is empty
+    cameraOne.result = result
+    estimatedPose = estimator.update()
+    assert estimatedPose is None
+    # Expect the old timestamp to still be here
+    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
 
-#     # Set new field layout -- right after, the pose cache timestamp should be -1
-#     estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
-#     assertEquals(-1, estimator._poseCacheTimestampSeconds)
-#     # Update should cache the current timestamp (20) again
-#     cameraOne.result = result
-#     estimatedPose = estimator.update()
-#     assertEquals(20, estimatedPose.timestampSeconds, 0.01)
-#     assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    # Set new field layout -- right after, the pose cache timestamp should be -1
+    estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
+    assertEquals(-1, estimator._poseCacheTimestampSeconds)
+    # Update should cache the current timestamp (20) again
+    cameraOne.result = result
+    estimatedPose = estimator.update()
+
+    assert estimatedPose is not None
+
+    assertEquals(20, estimatedPose.timestampSeconds, 0.01)
+    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
 
 
-# def assertEquals(expected, actual, epsilon=0.0):
-#     assert abs(expected - actual) <= epsilon
+def assertEquals(expected, actual, epsilon=0.0):
+    assert abs(expected - actual) <= epsilon

photon-lib/src/generate/photonlib.json.in

@@ -9,7 +9,47 @@
     "https://maven.photonvision.org/repository/snapshots"
   ],
   "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
-  "jniDependencies": [],
+  "jniDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibc",
+      "artifactId": "wpilibc-cpp",
+      "version": "${wpilib_version}",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "${photon_version}",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-jni",
+      "version": "${photon_version}",
+      "skipInvalidPlatforms": true,
+      "isJar": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
   "cppDependencies": [
     {
       "groupId": "org.photonvision",

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

@@ -32,9 +32,7 @@ import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.numbers.*;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanSubscriber;
-import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoubleArraySubscriber;
-import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
@@ -53,6 +51,7 @@ import java.util.stream.Collectors;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.common.networktables.PacketSubscriber;
 import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.timesync.TimeSyncSingleton;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera implements AutoCloseable {
@@ -63,13 +62,6 @@ public class PhotonCamera implements AutoCloseable {
     PacketSubscriber<PhotonPipelineResult> resultSubscriber;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
-    DoublePublisher latencyMillisEntry;
-    BooleanPublisher hasTargetEntry;
-    DoublePublisher targetPitchEntry;
-    DoublePublisher targetYawEntry;
-    DoublePublisher targetAreaEntry;
-    DoubleArrayPublisher targetPoseEntry;
-    DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
@@ -85,13 +77,6 @@ public class PhotonCamera implements AutoCloseable {
         resultSubscriber.close();
         driverModePublisher.close();
         driverModeSubscriber.close();
-        latencyMillisEntry.close();
-        hasTargetEntry.close();
-        targetPitchEntry.close();
-        targetYawEntry.close();
-        targetAreaEntry.close();
-        targetPoseEntry.close();
-        targetSkewEntry.close();
         versionEntry.close();
         inputSaveImgEntry.close();
         outputSaveImgEntry.close();
@@ -110,12 +95,15 @@ public class PhotonCamera implements AutoCloseable {
 
     private static boolean VERSION_CHECK_ENABLED = true;
     private static long VERSION_CHECK_INTERVAL = 5;
-    private double lastVersionCheckTime = 0;
+    double lastVersionCheckTime = 0;
 
     private long prevHeartbeatValue = -1;
     private double prevHeartbeatChangeTime = 0;
     private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;
 
+    double prevTimeSyncWarnTime = 0;
+    private static final double WARN_DEBOUNCE_SEC = 5;
+
     public static void setVersionCheckEnabled(boolean enabled) {
         VERSION_CHECK_ENABLED = enabled;
     }
@@ -166,6 +154,9 @@ public class PhotonCamera implements AutoCloseable {
 
         HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount);
         InstanceCount++;
+
+        // HACK - start a TimeSyncServer, if we haven't yet.
+        TimeSyncSingleton.load();
     }
 
     /**
@@ -189,13 +180,12 @@ public class PhotonCamera implements AutoCloseable {
 
         List<PhotonPipelineResult> ret = new ArrayList<>();
 
+        // Grab the latest results. We don't care about the timestamps from NT - the metadata header has
+        // this, latency compensated by the Time Sync Client
         var changes = resultSubscriber.getAllChanges();
-
-        // 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.
         for (var c : changes) {
             var result = c.value;
-            result.setReceiveTimestampMicros(c.timestamp);
+            checkTimeSyncOrWarn(result);
             ret.add(result);
         }
 
@@ -213,21 +203,38 @@ public class PhotonCamera implements AutoCloseable {
     public PhotonPipelineResult getLatestResult() {
         verifyVersion();
 
+        // 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
         var ret = resultSubscriber.get();
 
         if (ret.timestamp == 0) return new PhotonPipelineResult();
 
         var result = ret.value;
 
-        // Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result
-        // contains a thing with time knowledge, set it here.
-        // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
-        // TODO: NT4 time sync is Not To Be Trusted, we should do something else?
-        result.setReceiveTimestampMicros(ret.timestamp);
+        checkTimeSyncOrWarn(result);
 
         return result;
     }
 
+    private void checkTimeSyncOrWarn(PhotonPipelineResult result) {
+        if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
+            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.",
+                        false);
+            }
+        } else {
+            // Got a valid packet, reset the last time
+            prevTimeSyncWarnTime = 0;
+        }
+    }
+
     /**
      * Returns whether the camera is in driver mode.
      *
@@ -373,7 +380,7 @@ public class PhotonCamera implements AutoCloseable {
         return cameraTable;
     }
 
-    private void verifyVersion() {
+    void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 
         if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
@@ -410,7 +417,7 @@ public class PhotonCamera implements AutoCloseable {
         // Check for connection status. Warn if disconnected.
         else if (!isConnected()) {
             DriverStation.reportWarning(
-                    "PhotonVision coprocessor at path " + path + " is not sending new data.", true);
+                    "PhotonVision coprocessor at path " + path + " is not sending new data.", false);
         }
 
         String versionString = versionEntry.get("");
@@ -425,7 +432,7 @@ public class PhotonCamera implements AutoCloseable {
                     "PhotonVision coprocessor at path "
                             + path
                             + " has not reported a message interface UUID - is your coprocessor's camera started?",
-                    true);
+                    false);
         } else if (!local_uuid.equals(remote_uuid)) {
             // Error on a verified version mismatch
             // But stay silent otherwise

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

@@ -408,7 +408,7 @@ public class PhotonPoseEstimator {
                             result.getTargets(),
                             PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
         } else {
-            // We can nver fall back on another multitag strategy
+            // We can never fall back on another multitag strategy
             return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
         }
     }

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

@@ -68,8 +68,8 @@ import org.photonvision.targeting.PnpResult;
 public class PhotonCameraSim implements AutoCloseable {
     private final PhotonCamera cam;
 
-    NTTopicSet ts = new NTTopicSet();
-    private long heartbeatCounter = 0;
+    protected NTTopicSet ts = new NTTopicSet();
+    private long heartbeatCounter = 1;
 
     /** This simulated camera's {@link SimCameraProperties} */
     public final SimCameraProperties prop;
@@ -553,9 +553,10 @@ public class PhotonCameraSim implements AutoCloseable {
                         heartbeatCounter,
                         now - (long) (latencyMillis * 1000),
                         now,
+                        // Pretend like we heard a pong recently
+                        1000L + (long) ((Math.random() - 0.5) * 50),
                         detectableTgts,
                         multitagResult);
-        ret.setReceiveTimestampMicros(now);
         return ret;
     }
 
@@ -605,6 +606,8 @@ public class PhotonCameraSim implements AutoCloseable {
 
         ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp);
         ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp);
-        ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp);
+
+        ts.heartbeatPublisher.set(heartbeatCounter, receiveTimestamp);
+        heartbeatCounter += 1;
     }
 }

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

@@ -38,6 +38,7 @@ import java.util.Map;
 import org.opencv.core.Core;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
+import org.opencv.core.MatOfByte;
 import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
@@ -99,11 +100,18 @@ public class VideoSimUtil {
      *
      * @param id The fiducial id of the desired tag
      */
-    public static Mat get36h11TagImage(int id) {
+    private static Mat get36h11TagImage(int id) {
         RawFrame frame = AprilTag.generate36h11AprilTagImage(id);
-        Mat result = new Mat(10, 10, CvType.CV_8UC1, frame.getData(), frame.getStride()).clone();
-        frame.close();
-        return result;
+
+        var buf = frame.getData();
+        byte[] arr = new byte[buf.remaining()];
+        buf.get(arr);
+        // frame.close();
+
+        var mat = new MatOfByte(arr).reshape(1, 10).submat(new Rect(0, 0, 10, 10));
+        mat.dump();
+
+        return mat;
     }
 
     /** Gets the points representing the marker(black square) corners. */

photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java

@@ -0,0 +1,52 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.timesync;
+
+import java.io.IOException;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncServer;
+
+/** Helper to hold a single TimeSyncServer instance with some default config */
+public class TimeSyncSingleton {
+    private static TimeSyncServer INSTANCE = null;
+
+    public static boolean load() {
+        if (INSTANCE == null) {
+            try {
+                if (!PhotonTargetingJniLoader.load()) {
+                    return false;
+                }
+            } catch (UnsatisfiedLinkError | IOException e) {
+                e.printStackTrace();
+                return false;
+            }
+
+            INSTANCE = new TimeSyncServer(5810);
+            INSTANCE.start();
+        }
+
+        return INSTANCE != null;
+    }
+}

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

@@ -25,6 +25,7 @@
 #include "photon/PhotonCamera.h"
 
 #include <hal/FRCUsageReporting.h>
+#include <net/TimeSyncServer.h>
 
 #include <string>
 #include <string_view>
@@ -59,6 +60,22 @@ inline constexpr std::string_view bfw =
     ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
     "\n\n";
 
+// bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want
+// to avoid calling this from static initialization
+static void InitTspServer() {
+  // We dont impose requirements about not calling the PhotonCamera constructor
+  // from different threads, so i guess we need this?
+  static std::mutex g_timeSyncServerMutex;
+  static bool g_timeSyncServerStarted{false};
+  static wpi::tsp::TimeSyncServer timesyncServer{5810};
+
+  std::lock_guard lock{g_timeSyncServerMutex};
+  if (!g_timeSyncServerStarted) {
+    timesyncServer.Start();
+    g_timeSyncServerStarted = true;
+  }
+}
+
 namespace photon {
 
 constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
@@ -110,6 +127,11 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
       cameraName(cameraName) {
   HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount);
   InstanceCount++;
+
+  // The Robot class is actually created here:
+  // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33
+  // so we should be fine to call this from the ctor
+  InitTspServer();
 }
 
 PhotonCamera::PhotonCamera(const std::string_view cameraName)

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

@@ -330,7 +330,8 @@ PhotonPipelineResult PhotonCameraSim::Process(
   heartbeatCounter++;
   return PhotonPipelineResult{
       PhotonPipelineMetadata{heartbeatCounter, 0,
-                             units::microsecond_t{latency}.to<int64_t>()},
+                             units::microsecond_t{latency}.to<int64_t>(),
+                             1000000},
       detectableTgts, multiTagResults};
 }
 void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {

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

@@ -26,7 +26,6 @@ package org.photonvision;
 
 import static org.junit.jupiter.api.Assertions.*;
 
-import edu.wpi.first.cscore.CameraServerCvJNI;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -34,10 +33,12 @@ 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.networktables.NetworkTableInstance;
+import edu.wpi.first.util.CombinedRuntimeLoader;
 import java.io.IOException;
 import java.util.List;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
+import org.opencv.core.Core;
 import org.photonvision.estimation.CameraTargetRelation;
 import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.RotTrlTransform3d;
@@ -77,7 +78,7 @@ public class OpenCVTest {
 
     @BeforeAll
     public static void setUp() throws IOException {
-        CameraServerCvJNI.forceLoad();
+        CombinedRuntimeLoader.loadLibraries(OpenCVTest.class, Core.NATIVE_LIBRARY_NAME);
 
         // NT live for debug purposes
         NetworkTableInstance.getDefault().startServer();
@@ -150,7 +151,7 @@ public class OpenCVTest {
         assertEquals(
                 actualRelation.camToTargPitch.getDegrees(),
                 pitchDiff.getDegrees()
-                        * Math.cos(yaw2d.getRadians()), // adjust for unaccounted perpsective distortion
+                        * Math.cos(yaw2d.getRadians()), // adjust for unaccounted perspective distortion
                 kRotDeltaDeg,
                 "2d pitch doesn't match 3d");
         assertEquals(

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

@@ -24,12 +24,48 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.photonvision.UnitTestUtils.waitForCondition;
+import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
+
+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.Timer;
+import java.io.IOException;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
 import org.photonvision.common.dataflow.structures.Packet;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncClient;
+import org.photonvision.jni.WpilibLoader;
+import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 class PhotonCameraTest {
+    @BeforeAll
+    public static void load_wpilib() {
+        WpilibLoader.loadLibraries();
+    }
+
+    @BeforeEach
+    public void setup() {
+        HAL.initialize(500, 0);
+    }
+
+    @AfterEach
+    public void teardown() {
+        HAL.shutdown();
+    }
+
     @Test
     public void testEmpty() {
         Assertions.assertDoesNotThrow(
@@ -40,4 +76,158 @@ class PhotonCameraTest {
                     PhotonPipelineResult.photonStruct.pack(packet, ret);
                 });
     }
+
+    // Just a smoketest for dev use -- don't run by default
+    @Test
+    public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IOException {
+        load_wpilib();
+        PhotonTargetingJniLoader.load();
+
+        HAL.initialize(500, 0);
+
+        NetworkTableInstance.getDefault().stopClient();
+        NetworkTableInstance.getDefault().startServer();
+
+        var camera = new PhotonCamera("Arducam_OV2311_USB_Camera");
+        PhotonCamera.setVersionCheckEnabled(false);
+
+        for (int i = 0; i < 5; i++) {
+            Thread.sleep(500);
+
+            var res = camera.getLatestResult();
+            var captureTime = res.getTimestampSeconds();
+            var now = Timer.getFPGATimestamp();
+
+            // expectTrue(captureTime < now);
+
+            System.out.println(
+                    "sequence "
+                            + res.metadata.sequenceID
+                            + " image capture "
+                            + captureTime
+                            + " received at "
+                            + res.getTimestampSeconds()
+                            + " now: "
+                            + NetworkTablesJNI.now() / 1e6
+                            + " time since last pong: "
+                            + res.metadata.timeSinceLastPong / 1e6);
+        }
+
+        HAL.shutdown();
+    }
+
+    private static Stream<Arguments> testNtOffsets() {
+        return Stream.of(
+                // various initializaiton orders
+                Arguments.of(1, 10, 30, 30),
+                Arguments.of(10, 2, 30, 30),
+                Arguments.of(10, 10, 30, 30),
+                // Reboot just the robot
+                Arguments.of(1, 1, 10, 30),
+                // Reboot just the coproc
+                Arguments.of(1, 1, 30, 10));
+    }
+
+    /**
+     * Try starting client before server and vice-versa, making sure that we never fail the version
+     * check
+     */
+    @ParameterizedTest
+    @MethodSource("testNtOffsets")
+    public void testRestartingRobotAndCoproc(
+            int robotStart, int coprocStart, int robotRestart, int coprocRestart) throws Throwable {
+        var robotNt = NetworkTableInstance.create();
+        var coprocNt = NetworkTableInstance.create();
+
+        robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
+        coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
+
+        TimeSyncClient tspClient = null;
+
+        var robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
+
+        // apparently need a PhotonCamera to hand down
+        var fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA");
+        var coprocSim = new PhotonCameraSim(fakePhotonCoprocCam);
+        coprocSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
+        coprocSim.prop.setFPS(30);
+        coprocSim.setMinTargetAreaPixels(20.0);
+
+        for (int i = 0; i < 20; i++) {
+            int seq = i + 1;
+
+            if (i == coprocRestart) {
+                System.out.println("Restarting coprocessor NT client");
+
+                fakePhotonCoprocCam.close();
+                coprocNt.close();
+                coprocNt = NetworkTableInstance.create();
+
+                coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
+
+                fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA");
+                coprocSim = new PhotonCameraSim(fakePhotonCoprocCam);
+                coprocSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
+                coprocSim.prop.setFPS(30);
+                coprocSim.setMinTargetAreaPixels(20.0);
+            }
+            if (i == robotRestart) {
+                System.out.println("Restarting robot NT server");
+
+                robotNt.close();
+                robotNt = NetworkTableInstance.create();
+                robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
+                robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
+            }
+
+            if (i == coprocStart || i == coprocRestart) {
+                coprocNt.setServer("127.0.0.1", 5940);
+                coprocNt.startClient4("testClient");
+
+                // PhotonCamera makes a server by default - connect to it
+                tspClient = new TimeSyncClient("127.0.0.1", 5810, 0.5);
+            }
+
+            if (i == robotStart || i == robotRestart) {
+                robotNt.startServer("networktables_random.json", "", 5941, 5940);
+            }
+
+            Thread.sleep(100);
+
+            if (i == Math.max(coprocStart, robotStart)) {
+                final var c = coprocNt;
+                final var r = robotNt;
+                waitForCondition("Coproc connection", () -> c.getConnections().length == 1);
+                waitForCondition("Rio connection", () -> r.getConnections().length == 1);
+            }
+
+            var result1 = new PhotonPipelineResult();
+            result1.metadata.captureTimestampMicros = seq * 100;
+            result1.metadata.publishTimestampMicros = seq * 150;
+            result1.metadata.sequenceID = seq;
+            if (tspClient != null) {
+                result1.metadata.timeSinceLastPong = tspClient.getPingMetadata().timeSinceLastPong();
+            } else {
+                result1.metadata.timeSinceLastPong = Long.MAX_VALUE;
+            }
+
+            coprocSim.submitProcessedFrame(result1, NetworkTablesJNI.now());
+            coprocNt.flush();
+
+            if (i > robotStart && i > coprocStart) {
+                var ret = waitForSequenceNumber(robotCamera, seq);
+                System.out.println(ret);
+            }
+
+            // force verifyVersion to do checks
+            robotCamera.lastVersionCheckTime = -100;
+            robotCamera.prevTimeSyncWarnTime = -100;
+            assertDoesNotThrow(robotCamera::verifyVersion);
+        }
+
+        coprocSim.close();
+        coprocNt.close();
+        robotNt.close();
+        tspClient.stop();
+    }
 }

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

@@ -64,8 +64,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        11 * 1000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -130,7 +131,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
@@ -150,8 +150,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        4000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -217,8 +218,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
 
-        cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6));
-
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
                         aprilTags,
@@ -240,8 +239,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        17000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -306,7 +306,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -330,8 +329,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        1000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -396,7 +396,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -412,8 +411,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        7000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -478,7 +478,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6));
 
         estimatedPose = estimator.update(cameraOne.result);
         pose = estimatedPose.get().estimatedPose;
@@ -495,8 +494,9 @@ class PhotonPoseEstimatorTest {
         var result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        20000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -519,7 +519,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        result.setReceiveTimestampMicros((long) (20 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -529,7 +528,7 @@ class PhotonPoseEstimatorTest {
 
         // Empty result, expect empty result
         cameraOne.result = new PhotonPipelineResult();
-        cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
+        cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
         Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
         assertFalse(estimatedPose.isPresent());
 
@@ -563,8 +562,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        20 * 1000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -629,7 +629,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8))))); // 3 3 3 ambig .4
-        cameraOne.result.setReceiveTimestampMicros(20 * 1000000);
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(

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

@@ -0,0 +1,74 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import java.util.function.BooleanSupplier;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+public class UnitTestUtils {
+    static void waitForCondition(String name, BooleanSupplier condition) {
+        // wait up to 1 second for satisfaction
+        for (int i = 0; i < 100; i++) {
+            if (condition.getAsBoolean()) {
+                System.out.println(name + " satisfied on iteration " + i);
+                return;
+            }
+
+            try {
+                Thread.sleep(60);
+            } catch (InterruptedException e) {
+                e.printStackTrace();
+                fail(e);
+            }
+        }
+        throw new RuntimeException(name + " was never satisfied");
+    }
+
+    static PhotonPipelineResult waitForSequenceNumber(PhotonCamera camera, int seq) {
+        assertTrue(camera.heartbeatEntry.getTopic().getHandle() != 0);
+
+        System.out.println(
+                "Waiting for seq=" + seq + " on " + camera.heartbeatEntry.getTopic().getName());
+        // wait up to 1 second for a new result
+        for (int i = 0; i < 100; i++) {
+            var res = camera.getLatestResult();
+            System.out.println(res);
+            if (res.metadata.sequenceID == seq) {
+                return res;
+            }
+
+            try {
+                Thread.sleep(60);
+            } catch (InterruptedException e) {
+                e.printStackTrace();
+                fail(e);
+            }
+        }
+        throw new RuntimeException("Never saw sequence number " + seq);
+    }
+}

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

@@ -27,9 +27,12 @@ package org.photonvision;
 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 static org.photonvision.UnitTestUtils.waitForSequenceNumber;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -39,19 +42,25 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.stream.Stream;
-import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 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;
+import org.photonvision.jni.WpilibLoader;
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.simulation.VisionTargetSim;
@@ -61,6 +70,46 @@ class VisionSystemSimTest {
     private static final double kTrlDelta = 0.005;
     private static final double kRotDeltaDeg = 0.25;
 
+    NetworkTableInstance inst;
+
+    @BeforeAll
+    public static void setUp() {
+        WpilibLoader.loadLibraries();
+        try {
+            if (!PhotonTargetingJniLoader.load()) fail();
+        } catch (UnsatisfiedLinkError | IOException e) {
+            e.printStackTrace();
+            fail(e);
+        }
+
+        OpenCVHelp.forceLoadOpenCV();
+    }
+
+    @BeforeEach
+    public void init() {
+        // // No version check for testing
+        // PhotonCamera.setVersionCheckEnabled(false);
+    }
+
+    @BeforeEach
+    public void setup() {
+        HAL.initialize(500, 0);
+
+        inst = NetworkTableInstance.create();
+        inst.stopClient();
+        inst.stopServer();
+        inst.startLocal();
+        SmartDashboard.setNetworkTableInstance(inst);
+    }
+
+    @AfterEach
+    public void teardown() {
+        inst.close();
+        inst = null;
+
+        HAL.shutdown();
+    }
+
     @Test
     public void testEmpty() {
         Assertions.assertDoesNotThrow(
@@ -74,41 +123,12 @@ class VisionSystemSimTest {
                 });
     }
 
-    @BeforeAll
-    public static void setUp() {
-        // NT live for debug purposes
-        NetworkTableInstance.getDefault().startServer();
-
-        // No version check for testing
-        PhotonCamera.setVersionCheckEnabled(false);
-    }
-
-    @AfterAll
-    public static void shutDown() {}
-
-    // @ParameterizedTest
-    // @ValueSource(doubles = {5, 10, 15, 20, 25, 30})
-    // public void testDistanceAligned(double dist) {
-    //     final var targetPose = new Pose2d(new Translation2d(15.98, 0), new Rotation2d());
-    //     var sysUnderTest =
-    //             new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
-    //     sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 1.0, 1.0));
-
-    //     final var robotPose = new Pose2d(new Translation2d(35 - dist, 0), new Rotation2d());
-    //     sysUnderTest.processFrame(robotPose);
-
-    //     var result = sysUnderTest.cam.getLatestResult();
-
-    //     assertTrue(result.hasTargets());
-    //     assertEquals(result.getBestTarget().getCameraToTarget().getTranslation().getNorm(), dist);
-    // }
-
     @Test
     public void testVisibilityCupidShuffle() {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
@@ -117,42 +137,51 @@ class VisionSystemSimTest {
         // To the right, to the right
         var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70));
         visionSysSim.update(robotPose);
+
         assertFalse(camera.getLatestResult().hasTargets());
 
         // To the right, to the right
         robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95));
         visionSysSim.update(robotPose);
+
         assertFalse(camera.getLatestResult().hasTargets());
 
         // To the left, to the left
         robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90));
         visionSysSim.update(robotPose);
+
         assertFalse(camera.getLatestResult().hasTargets());
 
         // To the left, to the left
         robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65));
         visionSysSim.update(robotPose);
+
         assertFalse(camera.getLatestResult().hasTargets());
 
         // now kick, now kick
         robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
+
+        waitForSequenceNumber(camera, 5);
         assertTrue(camera.getLatestResult().hasTargets());
 
         // now kick, now kick
         robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5));
         visionSysSim.update(robotPose);
+
         assertTrue(camera.getLatestResult().hasTargets());
 
         // now walk it by yourself
         robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179));
         visionSysSim.update(robotPose);
+
         assertFalse(camera.getLatestResult().hasTargets());
 
         // now walk it by yourself
         visionSysSim.adjustCamera(
                 cameraSim, new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI)));
         visionSysSim.update(robotPose);
+
         assertTrue(camera.getLatestResult().hasTargets());
     }
 
@@ -161,7 +190,7 @@ class VisionSystemSimTest {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
@@ -169,12 +198,14 @@ class VisionSystemSimTest {
 
         var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertTrue(camera.getLatestResult().hasTargets());
+
+        assertTrue(waitForSequenceNumber(camera, 1).hasTargets());
 
         visionSysSim.adjustCamera( // vooop selfie stick
                 cameraSim, new Transform3d(new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI)));
         visionSysSim.update(robotPose);
-        assertFalse(camera.getLatestResult().hasTargets());
+
+        assertFalse(waitForSequenceNumber(camera, 2).hasTargets());
     }
 
     @Test
@@ -184,7 +215,7 @@ class VisionSystemSimTest {
         var robotToCamera =
                 new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI / 4, 0));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, robotToCamera);
         cameraSim.prop.setCalibration(1234, 1234, Rotation2d.fromDegrees(80));
@@ -192,12 +223,15 @@ class VisionSystemSimTest {
 
         var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertTrue(camera.getLatestResult().hasTargets());
 
-        // Pitched back camera should mean target goes out of view below the robot as distance increases
+        assertTrue(waitForSequenceNumber(camera, 1).hasTargets());
+
+        // Pitched back camera should mean target goes out of view below the robot as
+        // distance increases
         robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertFalse(camera.getLatestResult().hasTargets());
+
+        assertFalse(waitForSequenceNumber(camera, 2).hasTargets());
     }
 
     @Test
@@ -205,7 +239,7 @@ class VisionSystemSimTest {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
@@ -214,11 +248,13 @@ class VisionSystemSimTest {
 
         var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertTrue(camera.getLatestResult().hasTargets());
+
+        assertTrue(waitForSequenceNumber(camera, 1).hasTargets());
 
         robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertFalse(camera.getLatestResult().hasTargets());
+
+        assertFalse(waitForSequenceNumber(camera, 2).hasTargets());
     }
 
     @Test
@@ -226,7 +262,7 @@ class VisionSystemSimTest {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
@@ -236,20 +272,22 @@ class VisionSystemSimTest {
 
         var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertTrue(camera.getLatestResult().hasTargets());
+
+        assertTrue(waitForSequenceNumber(camera, 1).hasTargets());
 
         robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
         visionSysSim.update(robotPose);
-        assertFalse(camera.getLatestResult().hasTargets());
+
+        assertFalse(waitForSequenceNumber(camera, 2).hasTargets());
     }
 
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23})
-    public void testYawAngles(double testYaw) {
+    public void testYawAngles(double testYaw) throws InterruptedException {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
@@ -259,7 +297,8 @@ class VisionSystemSimTest {
         // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+)
         var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(testYaw));
         visionSysSim.update(robotPose);
-        var res = camera.getLatestResult();
+
+        var res = waitForSequenceNumber(camera, 1);
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
         assertEquals(testYaw, tgt.getYaw(), kRotDeltaDeg);
@@ -267,12 +306,12 @@ class VisionSystemSimTest {
 
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
-    public void testPitchAngles(double testPitch) {
+    public void testPitchAngles(double testPitch) throws InterruptedException {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4));
         final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0));
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(120));
@@ -284,12 +323,15 @@ class VisionSystemSimTest {
                 cameraSim,
                 new Transform3d(
                         new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
+
         visionSysSim.update(robotPose);
-        var res = camera.getLatestResult();
+        var res = waitForSequenceNumber(camera, 1);
+        System.out.println("Got result: " + res);
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
 
-        // Since the camera is level with the target, a positive-upward point will mean the target is in
+        // Since the camera is level with the target, a positive-upward point will mean
+        // the target is in
         // the
         // lower half of the image
         // which should produce negative pitch.
@@ -297,7 +339,8 @@ class VisionSystemSimTest {
     }
 
     private static Stream<Arguments> testDistanceCalcArgs() {
-        // Arbitrary and fairly random assortment of distances, camera pitches, and heights
+        // Arbitrary and fairly random assortment of distances, camera pitches, and
+        // heights
         return Stream.of(
                 Arguments.of(5, -15.98, 0),
                 Arguments.of(6, -15.98, 1),
@@ -321,7 +364,8 @@ class VisionSystemSimTest {
     @ParameterizedTest
     @MethodSource("testDistanceCalcArgs")
     public void testDistanceCalc(double testDist, double testPitch, double testHeight) {
-        // Assume dist along ground and tgt height the same. Iterate over other parameters.
+        // Assume dist along ground and tgt height the same. Iterate over other
+        // parameters.
 
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98));
@@ -335,7 +379,7 @@ class VisionSystemSimTest {
         var visionSysSim =
                 new VisionSystemSim(
                         "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(160));
@@ -345,11 +389,15 @@ class VisionSystemSimTest {
 
         visionSysSim.update(robotPose);
 
-        // Note that target 2d yaw/pitch accuracy is hindered by two factors in photonvision:
-        // 1. These are calculated with the average of the minimum area rectangle, which does not
+        // Note that target 2d yaw/pitch accuracy is hindered by two factors in
+        // photonvision:
+        // 1. These are calculated with the average of the minimum area rectangle, which
+        // does not
         // actually find the target center because of perspective distortion.
-        // 2. Yaw and pitch are calculated separately which gives incorrect pitch values.
-        var res = camera.getLatestResult();
+        // 2. Yaw and pitch are calculated separately which gives incorrect pitch
+        // values.
+
+        var res = waitForSequenceNumber(camera, 1);
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
         assertEquals(0.0, tgt.getYaw(), 0.5);
@@ -375,7 +423,7 @@ class VisionSystemSimTest {
                 new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI));
 
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
@@ -450,7 +498,9 @@ class VisionSystemSimTest {
 
         var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25));
         visionSysSim.update(robotPose);
-        var res = camera.getLatestResult();
+
+        var res = waitForSequenceNumber(camera, 1);
+
         assertTrue(res.hasTargets());
         List<PhotonTrackedTarget> tgtList;
         tgtList = res.getTargets();
@@ -460,7 +510,7 @@ class VisionSystemSimTest {
     @Test
     public void testPoseEstimation() {
         var visionSysSim = new VisionSystemSim("Test");
-        var camera = new PhotonCamera("camera");
+        var camera = new PhotonCamera(inst, "camera");
         var cameraSim = new PhotonCameraSim(camera);
         visionSysSim.addCamera(cameraSim, new Transform3d());
         cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
@@ -479,11 +529,12 @@ class VisionSystemSimTest {
                 new VisionTargetSim(tagList.get(0).pose, TargetModel.kAprilTag16h5, 0));
 
         visionSysSim.update(robotPose);
+
         var results =
                 VisionEstimation.estimateCamPosePNP(
                                 camera.getCameraMatrix().get(),
                                 camera.getDistCoeffs().get(),
-                                camera.getLatestResult().getTargets(),
+                                waitForSequenceNumber(camera, 1).getTargets(),
                                 layout,
                                 TargetModel.kAprilTag16h5)
                         .get();
@@ -499,6 +550,7 @@ class VisionSystemSimTest {
                 new VisionTargetSim(tagList.get(2).pose, TargetModel.kAprilTag16h5, 2));
 
         visionSysSim.update(robotPose);
+
         results =
                 VisionEstimation.estimateCamPosePNP(
                                 camera.getCameraMatrix().get(),

photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java

@@ -1,79 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) PhotonVision
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-package org.photonvision.estimation;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import java.io.IOException;
-import org.junit.jupiter.api.BeforeAll;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-
-public class ApriltagWorkbenchTest {
-    @BeforeAll
-    public static void setUp() {
-        // No version check for testing
-        PhotonCamera.setVersionCheckEnabled(false);
-    }
-
-    // @Test
-    public void testMeme() throws IOException, InterruptedException {
-        NetworkTableInstance instance = NetworkTableInstance.getDefault();
-        instance.stopServer();
-        // set the NT server if simulating this code.
-        // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]"
-        // for coprocessor
-        instance.setServer("localhost");
-        instance.startClient4("myRobot");
-
-        var robotToCamera = new Transform3d();
-        var cam = new PhotonCamera("WPI2023");
-        var tagLayout =
-                AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
-
-        var pe =
-                new PhotonPoseEstimator(
-                        tagLayout,
-                        PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-                        robotToCamera);
-
-        var field = new Field2d();
-        SmartDashboard.putData(field);
-
-        while (!Thread.interrupted()) {
-            Thread.sleep(500);
-
-            for (var change : cam.getAllUnreadResults()) {
-                var ret = pe.update(change);
-                System.out.println(ret);
-                field.setRobotPose(ret.get().estimatedPose.toPose2d());
-            }
-        }
-    }
-}

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

@@ -0,0 +1,54 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <gtest/gtest.h>
+#include <hal/HAL.h>
+#include <net/TimeSyncClient.h>
+#include <net/TimeSyncServer.h>
+
+#include "photon/PhotonCamera.h"
+
+TEST(TimeSyncProtocolTest, Smoketest) {
+  using namespace wpi::tsp;
+  using namespace std::chrono_literals;
+
+  // start a server implicitly
+  photon::PhotonCamera camera{"camera"};
+
+  TimeSyncClient client{"127.0.0.1", 5810, 100ms};
+  client.Start();
+
+  for (int i = 0; i < 10; i++) {
+    std::this_thread::sleep_for(100ms);
+    TimeSyncClient::Metadata m = client.GetMetadata();
+
+    // give us time to warm up
+    if (i > 5) {
+      EXPECT_TRUE(m.rtt2 > 0);
+      EXPECT_TRUE(m.pongsReceived > 0);
+    }
+  }
+
+  client.Stop();
+}

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

@@ -86,7 +86,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
 
   photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
@@ -147,7 +147,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(17_s);
 
   photon::PhotonPoseEstimator estimator(
@@ -196,7 +196,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
 
   photon::PhotonPoseEstimator estimator(aprilTags,
@@ -247,7 +247,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
 
   photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
@@ -287,7 +287,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
           0.4, corners, detectedCorners}};
 
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
+      std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
 
   // std::optional<photon::EstimatedRobotPose> estimatedPose;
@@ -333,7 +334,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
 
   photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
@@ -387,7 +388,7 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
 
   // empty input, expect empty out
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000},
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
       std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
 
@@ -400,7 +401,7 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
 
   // Set result, and update -- expect present and timestamp to be 15
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}};
+      photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
   cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
 
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -422,7 +423,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
   std::vector<photon::PhotonTrackedTarget> targets{};
 
   auto testResult = photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt};
+      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
   testResult.SetReceiveTimestamp(units::second_t(11));
 
   auto test2 = testResult;

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

@@ -22,10 +22,14 @@
  * SOFTWARE.
  */
 
+#include <hal/HAL.h>
+
 #include "gtest/gtest.h"
 
 int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
   ::testing::InitGoogleTest(&argc, argv);
   int ret = RUN_ALL_TESTS();
+  HAL_Shutdown();
   return ret;
 }

photon-serde/README.md

@@ -15,7 +15,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 serde interface](src/serde/pnpresult_struct.h): This is a template specilization for converting the user class to/from bytes
+- [The serde interface](src/serde/pnpresult_struct.h): This is a template specialization for converting the user class to/from bytes
 
 ## Prior art
 

photon-serde/generate_messages.py

@@ -46,6 +46,7 @@ class MessageType(TypedDict):
     # C++ helpers
     cpp_include: str
     # python shim types
+    python_encode_shim: str
     python_decode_shim: str
     # Java import name
     java_import: str

photon-serde/message_data_types.yaml

@@ -5,29 +5,35 @@ bool:
   java_type: bool
   cpp_type: bool
   java_decode_method: decodeBoolean
+  java_encode_shim: encodeBoolean
 int16:
   len: 2
   java_type: short
   cpp_type: int16_t
   java_decode_method: decodeShort
   java_list_decode_method: decodeShortList
+  java_encode_shim: encodeShort
 int32:
   len: 4
   java_type: int
   cpp_type: int32_t
   java_decode_method: decodeInt
+  java_encode_shim: encodeInt
 int64:
   len: 8
   java_type: long
   cpp_type: int64_t
   java_decode_method: decodeLong
+  java_encode_shim: encodeLong
 float32:
   len: 4
   java_type: float
   cpp_type: float
   java_decode_method: decodeFloat
+  java_encode_shim: encodeFloat
 float64:
   len: 8
   java_type: double
   cpp_type: double
   java_decode_method: decodeDouble
+  java_encode_shim: encodeDouble

photon-serde/messages.yaml

@@ -7,6 +7,8 @@
     type: int64
   - name: publishTimestampMicros
     type: int64
+  - name: timeSinceLastPong
+    type: int64
 
 - name: Transform3d
   shimmed: True
@@ -15,6 +17,7 @@
   cpp_type: frc::Transform3d
   cpp_include: "<frc/geometry/Transform3d.h>"
   python_decode_shim: packet.decodeTransform
+  python_encode_shim: encodeTransform
   java_import: edu.wpi.first.math.geometry.Transform3d
   # shim since we expect fields to at least exist
   fields: []

photon-serde/templates/ThingSerde.py.jinja

@@ -21,6 +21,7 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 class {{ name }}Serde:
 
@@ -28,6 +29,34 @@ class {{ name }}Serde:
     MESSAGE_VERSION = "{{ message_hash }}"
     MESSAGE_FORMAT = "{{ message_fmt }}"
 
+    @staticmethod
+    def pack(value: '{{ name }}' ) -> 'Packet':
+        ret = Packet()
+{% for field in fields -%}
+{%- if field.type | is_shimmed %}
+        ret.{{ get_message_by_name(field.type).python_encode_shim}}(value.{{ field.name }})
+{%- elif field.optional == True %}
+        # {{ field.name }} is optional! it better not be a VLA too
+        ret.encodeOptional(value.{{ field.name }}, {{ field.type }}.photonStruct)
+{%- elif field.vla == True and not field.type | is_intrinsic %}
+        # {{ field.name }} is a custom VLA!
+        ret.encodeList(value.{{ field.name }}, {{ field.type }}.photonStruct)
+{%- elif field.vla == True and field.type | is_intrinsic %}
+        # {{ field.name }} is a custom VLA!
+        ret.encode{{ type_map[field.type].java_type.title() }}List(value.{{ field.name }})
+{%- elif field.type | is_intrinsic %}
+        # {{ field.name }} is of intrinsic type {{ field.type }}
+        ret.{{ type_map[field.type].java_encode_shim }}(value.{{field.name}})
+{%- else %}
+        # {{ field.name }} is of non-intrinsic type {{ field.type }}
+        ret.encodeBytes({{ field.type }}.photonStruct.pack(value.{{field.name}}).getData())
+{%- endif %}
+{%- if not loop.last %}
+{% endif -%}
+{% endfor%}
+        return ret
+
+
     @staticmethod
     def unpack(packet: 'Packet') -> '{{ name }}':
         ret = {{ name }}()

photon-server/build.gradle

@@ -11,6 +11,9 @@ apply from: "${rootDir}/shared/common.gradle"
 dependencies {
     implementation project(':photon-core')
 
+    // Zip
+    implementation 'org.zeroturnaround:zt-zip:1.14'
+
     // Needed for Javalin Runtime Logging
     implementation "org.slf4j:slf4j-simple:2.0.7"
 }
@@ -22,6 +25,10 @@ application {
     mainClass = 'org.photonvision.Main'
 }
 
+jar {
+    from file("$rootDir/LICENSE")
+}
+
 shadowJar {
     archiveBaseName = "photonvision"
     archiveVersion = project.version as String
@@ -71,7 +78,7 @@ remotes {
 task findDeployTarget {
     doLast {
         if(project.hasProperty('tgtIP')){
-            //If user specificed IP, default to using the PI profile
+            //If user specified IP, default to using the PI profile
             // but adjust hostname to match the provided IP address
             findDeployTarget.ext.rmt = remotes.pi
             findDeployTarget.ext.rmt.host=tgtIP

photon-server/settings.gradle

@@ -1 +0,0 @@
-rootProject.name = 'photon-server'

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

@@ -17,6 +17,7 @@
 
 package org.photonvision;
 
+import edu.wpi.first.hal.HAL;
 import java.io.IOException;
 import java.nio.file.Files;
 import java.nio.file.Path;
@@ -32,6 +33,7 @@ import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.hardware.PiVersion;
 import org.photonvision.common.hardware.Platform;
+import org.photonvision.common.logging.KernelLogLogger;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
@@ -39,6 +41,7 @@ import org.photonvision.common.logging.PvCSCoreLogger;
 import org.photonvision.common.networking.NetworkManager;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.jni.RknnDetectorJNI;
 import org.photonvision.mrcal.MrCalJNILoader;
 import org.photonvision.raspi.LibCameraJNILoader;
@@ -379,7 +382,25 @@ public class Main {
             logger.error("Failed to load native libraries!", e);
             System.exit(1);
         }
-        logger.info("Native libraries loaded.");
+        logger.info("WPI JNI libraries loaded.");
+
+        try {
+            boolean success = PhotonTargetingJniLoader.load();
+
+            if (!success) {
+                logger.error("Failed to load native libraries! Giving up :(");
+                System.exit(1);
+            }
+        } catch (Exception e) {
+            logger.error("Failed to load photon-targeting JNI!", e);
+            System.exit(1);
+        }
+        logger.info("photon-targeting JNI libraries loaded.");
+
+        if (!HAL.initialize(500, 0)) {
+            logger.error("Failed to initialize the HAL! Giving up :(");
+            System.exit(1);
+        }
 
         try {
             if (Platform.isRaspberryPi()) {
@@ -417,6 +438,10 @@ public class Main {
         Logger.setLevel(LogGroup.General, logLevel);
         logger.info("Logging initialized in debug mode.");
 
+        // Add Linux kernel log->Photon logger
+        KernelLogLogger.getInstance();
+
+        // Add CSCore->Photon logger
         PvCSCoreLogger.getInstance();
 
         logger.debug("Loading ConfigManager...");
@@ -438,6 +463,7 @@ public class Main {
         logger.debug("Loading NetworkTablesManager...");
         NetworkTablesManager.getInstance()
                 .setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
+        NetworkTablesManager.getInstance().registerTimedTasks();
 
         if (isSmoketest) {
             logger.info("PhotonVision base functionality loaded -- smoketest complete");

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

@@ -53,6 +53,7 @@ import org.photonvision.common.util.file.ProgramDirectoryUtilities;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.CameraQuirk;
 import org.photonvision.vision.processes.VisionModuleManager;
+import org.zeroturnaround.zip.ZipUtil;
 
 public class RequestHandler {
     // Treat all 2XX calls as "INFO"
@@ -422,20 +423,34 @@ public class RequestHandler {
         try {
             ShellExec shell = new ShellExec();
             var tempPath = Files.createTempFile("photonvision-journalctl", ".txt");
-            shell.executeBashCommand("journalctl -u photonvision.service > " + tempPath.toAbsolutePath());
+            var tempPath2 = Files.createTempFile("photonvision-kernelogs", ".txt");
+            shell.executeBashCommand(
+                    "journalctl -u photonvision.service > "
+                            + tempPath.toAbsolutePath()
+                            + " && journalctl -k > "
+                            + tempPath2.toAbsolutePath());
 
             while (!shell.isOutputCompleted()) {
                 // TODO: add timeout
             }
 
             if (shell.getExitCode() == 0) {
-                // Wrote to the temp file! Add it to the ctx
-                var stream = new FileInputStream(tempPath.toFile());
-                ctx.contentType("text/plain");
-                ctx.header("Content-Disposition", "attachment; filename=\"photonvision-journalctl.txt\"");
-                ctx.status(200);
+                // Wrote to the temp file! Zip and yeet it to the client
+
+                var out = Files.createTempFile("photonvision-logs", "zip").toFile();
+
+                try {
+                    ZipUtil.packEntries(new File[] {tempPath.toFile(), tempPath2.toFile()}, out);
+                } catch (Exception e) {
+                    e.printStackTrace();
+                }
+
+                var stream = new FileInputStream(out);
+                ctx.contentType("application/zip");
+                ctx.header("Content-Disposition", "attachment; filename=\"photonvision-logs.zip\"");
                 ctx.result(stream);
-                logger.info("Uploading settings with size " + stream.available());
+                ctx.status(200);
+                logger.info("Outputting log ZIP with size " + stream.available());
             } else {
                 ctx.status(500);
                 ctx.result("The journalctl service was unable to export logs");

photon-targeting/build.gradle

@@ -1,7 +1,3 @@
-plugins {
-    id 'edu.wpi.first.WpilibTools' version '1.3.0'
-}
-
 ext {
     nativeName = "photontargeting"
 }
@@ -9,8 +5,10 @@ ext {
 apply plugin: 'cpp'
 apply plugin: 'google-test-test-suite'
 apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: 'edu.wpi.first.WpilibTools'
 apply plugin: 'edu.wpi.first.GradleJni'
 
+ext.licenseFile = file("$rootDir/LICENSE")
 apply from: "${rootDir}/shared/config.gradle"
 apply from: "${rootDir}/shared/javacommon.gradle"
 
@@ -93,12 +91,23 @@ model {
             nativeUtils.useRequiredLibrary(it, "wpiutil_shared")
             nativeUtils.useRequiredLibrary(it, "wpinet_shared")
             nativeUtils.useRequiredLibrary(it, "ntcore_shared")
+            nativeUtils.useRequiredLibrary(it, "wpimath_shared")
         }
 
         all {
             binaries.withType(SharedLibraryBinarySpec) { binary ->
                 // check that we're building for the platform (per PArchOverride/wpilib plat detection)
-                if (binary.targetPlatform.name == jniPlatform) {
+                def platName = jniPlatform
+                def realWpilibName = wpilibNativeName;
+
+                if (jniPlatform.equals("osxarm64") || jniPlatform.equals("osxx86-64")) {
+                    // native-utils calls this osxuniversal
+                    platName = "osxuniversal";
+                    realWpilibName = "osxuniversal";
+                }
+
+                if (binary.targetPlatform.name == platName) {
+
 
                     // only include release binaries (hard coded for now)
                     def isDebug = binary.buildType.name.contains('debug')
@@ -106,7 +115,7 @@ model {
                         syncOutputsFolder {
                             // Just shove the shared library into the root of the jar output by photon-targeting:jar
                             from(binary.sharedLibraryFile) {
-                                into "nativelibraries/${wpilibNativeName}/"
+                                into "nativelibraries/${realWpilibName}/"
                             }
                             // And (not sure if this is a hack) make the jar task depend on the build task
                             dependsOn binary.identifier.projectScopedName
@@ -206,8 +215,12 @@ def nativeTasks = wpilibTools.createExtractionTasks {
 
 nativeTasks.addToSourceSetResources(sourceSets.test)
 
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")

photon-targeting/settings.gradle

@@ -1,6 +0,0 @@
-pluginManagement {
-    repositories {
-        mavenLocal()
-        gradlePluginPortal()
-    }
-}

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

@@ -0,0 +1,86 @@
+/*
+ * 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;
+
+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;
+import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.jni.ArmFeedforwardJNI;
+import edu.wpi.first.math.jni.DAREJNI;
+import edu.wpi.first.math.jni.EigenJNI;
+import edu.wpi.first.math.jni.Ellipse2dJNI;
+import edu.wpi.first.math.jni.Pose3dJNI;
+import edu.wpi.first.math.jni.StateSpaceUtilJNI;
+import edu.wpi.first.math.jni.TrajectoryUtilJNI;
+import edu.wpi.first.net.WPINetJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+
+public class WpilibLoader {
+    private static boolean has_loaded = false;
+    public static boolean loadLibraries() {
+        if (has_loaded) return true;
+
+        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
+        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
+        CameraServerJNI.Helper.setExtractOnStaticLoad(false);
+        OpenCvLoader.Helper.setExtractOnStaticLoad(false);
+        JNIWrapper.Helper.setExtractOnStaticLoad(false);
+        WPINetJNI.Helper.setExtractOnStaticLoad(false);
+        AprilTagJNI.Helper.setExtractOnStaticLoad(false);
+
+        // wpimathjni is a bit odd, it's all in the wpimathjni shared lib, but the java side stuff has
+        // been split.
+        ArmFeedforwardJNI.Helper.setExtractOnStaticLoad(false);
+        DAREJNI.Helper.setExtractOnStaticLoad(false);
+        EigenJNI.Helper.setExtractOnStaticLoad(false);
+        Ellipse2dJNI.Helper.setExtractOnStaticLoad(false);
+        Pose3dJNI.Helper.setExtractOnStaticLoad(false);
+        StateSpaceUtilJNI.Helper.setExtractOnStaticLoad(false);
+        TrajectoryUtilJNI.Helper.setExtractOnStaticLoad(false);
+
+        try {
+            CombinedRuntimeLoader.loadLibraries(
+                    WpilibLoader.class,
+                    "wpiutiljni",
+                    "wpilibc",
+                    "wpimathjni",
+                    "ntcorejni",
+                    "wpinetjni",
+                    "wpiHaljni",
+                    "wpi",
+                    "cscorejni",
+                    "apriltagjni");
+
+            CombinedRuntimeLoader.loadLibraries(WpilibLoader.class, Core.NATIVE_LIBRARY_NAME);
+
+            has_loaded = true;
+        } catch (IOException e) {
+            e.printStackTrace();
+            has_loaded = false;
+        }
+
+        return has_loaded;
+    }
+}

photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java

@@ -35,9 +35,9 @@ import edu.wpi.first.util.struct.Struct;
  */
 public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMetadata> {
     @Override
-    public final String getInterfaceUUID() { return "626e70461cbdb274fb43ead09c255f4e"; }
+    public final String getInterfaceUUID() { return "ac0a45f686457856fb30af77699ea356"; }
     @Override
-    public final String getSchema() { return "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"; }
+    public final String getSchema() { return "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;int64 timeSinceLastPong;"; }
     @Override
     public final String getTypeName() { return "PhotonPipelineMetadata"; }
 
@@ -57,6 +57,9 @@ public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMe
 
         // field publishTimestampMicros is of intrinsic type int64
         packet.encode((long) value.publishTimestampMicros);
+
+        // field timeSinceLastPong is of intrinsic type int64
+        packet.encode((long) value.timeSinceLastPong);
     }
 
     @Override
@@ -72,6 +75,9 @@ public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMe
         // publishTimestampMicros is of intrinsic type int64
         ret.publishTimestampMicros = packet.decodeLong();
 
+        // timeSinceLastPong is of intrinsic type int64
+        ret.timeSinceLastPong = packet.decodeLong();
+
         return ret;
     }
 

photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java

@@ -35,9 +35,9 @@ import edu.wpi.first.util.struct.Struct;
  */
 public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResult> {
     @Override
-    public final String getInterfaceUUID() { return "5eeaa293d0c69aea90eaddea786a2b3b"; }
+    public final String getInterfaceUUID() { return "4b2ff16a964b5e2bf04be0c1454d91c4"; }
     @Override
-    public final String getSchema() { return "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; }
+    public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; }
     @Override
     public final String getTypeName() { return "PhotonPipelineResult"; }
 
@@ -78,7 +78,7 @@ public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResu
     @Override
     public PacketSerde<?>[] getNestedPhotonMessages() {
         return new PacketSerde<?>[] {
-            PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct
+            MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct
         };
     }
 

photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp

@@ -28,6 +28,7 @@ void StructType::Pack(Packet& packet, const PhotonPipelineMetadata& value) {
   packet.Pack<int64_t>(value.sequenceID);
   packet.Pack<int64_t>(value.captureTimestampMicros);
   packet.Pack<int64_t>(value.publishTimestampMicros);
+  packet.Pack<int64_t>(value.timeSinceLastPong);
 }
 
 PhotonPipelineMetadata StructType::Unpack(Packet& packet) {
@@ -35,6 +36,7 @@ PhotonPipelineMetadata StructType::Unpack(Packet& packet) {
       .sequenceID = packet.Unpack<int64_t>(),
       .captureTimestampMicros = packet.Unpack<int64_t>(),
       .publishTimestampMicros = packet.Unpack<int64_t>(),
+      .timeSinceLastPong = packet.Unpack<int64_t>(),
   }};
 }
 

photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h

@@ -34,12 +34,12 @@ namespace photon {
 template <>
 struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineMetadata> {
   static constexpr std::string_view GetSchemaHash() {
-    return "626e70461cbdb274fb43ead09c255f4e";
+    return "ac0a45f686457856fb30af77699ea356";
   }
 
   static constexpr std::string_view GetSchema() {
     return "int64 sequenceID;int64 captureTimestampMicros;int64 "
-           "publishTimestampMicros;";
+           "publishTimestampMicros;int64 timeSinceLastPong;";
   }
 
   static photon::PhotonPipelineMetadata Unpack(photon::Packet& packet);

photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h

@@ -39,11 +39,11 @@ namespace photon {
 template <>
 struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineResult> {
   static constexpr std::string_view GetSchemaHash() {
-    return "5eeaa293d0c69aea90eaddea786a2b3b";
+    return "4b2ff16a964b5e2bf04be0c1454d91c4";
   }
 
   static constexpr std::string_view GetSchema() {
-    return "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e "
+    return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 "
            "metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 "
            "targets[?];optional "
            "MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b "