The definition of insanity is writing the same code twice and expecti…

…ng different results Same bug with implicit cast bullshit
PhotonVision · Oct 8, 2023 · 5ee7fdf · 5ee7fdf
1 parent 63279ef
commit 5ee7fdf
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 3 deletions.
diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
@@ -85,6 +85,16 @@ public void encode(byte src) {
         packetData[writePos++] = src;
     }
 
+    /**
+     * Encodes the short into the packet.
+     *
+     * @param src The short to encode.
+     */
+    public void encode(short src) {
+        packetData[writePos++] = (byte) (src >>> 8);
+        packetData[writePos++] = (byte) src;
+    }
+
     /**
      * Encodes the integer into the packet.
      *
@@ -196,4 +206,11 @@ public double[] decodeDoubleArray(int len) {
         }
         return ret;
     }
+
+    public short decodeShort() {
+        if (packetData.length < readPos + 1) {
+            return 0;
+        }
+        return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++]));
+    }
 }
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
@@ -41,7 +41,7 @@ public static MultiTargetPNPResults createFromPacket(Packet packet) {
         var results = PNPResults.createFromPacket(packet);
         var ids = new ArrayList<Integer>(MAX_IDS);
         for (int i = 0; i < MAX_IDS; i++) {
-            int targetId = (int) packet.decodeByte();
+            int targetId = (int) packet.decodeShort();
             if (targetId > -1) ids.add(targetId);
         }
         return new MultiTargetPNPResults(results, ids);
@@ -51,9 +51,9 @@ public void populatePacket(Packet packet) {
         estimatedPose.populatePacket(packet);
         for (int i = 0; i < MAX_IDS; i++) {
             if (i < fiducialIDsUsed.size()) {
-                packet.encode(fiducialIDsUsed.get(i).byteValue());
+                packet.encode((short) fiducialIDsUsed.get(i).byteValue());
             } else {
-                packet.encode(-1);
+                packet.encode((short) -1);
             }
         }
     }