Fixed C++ Struct Math and Bitwise Math

Corrected Line Tracking C++ Struct Math thanks to Jared Brogni
Corrected bitwise operation on 16bit angle value

Author: PseudoResonance

src/main/java/io/github/pseudoresonance/pixy2api/Pixy2.java

@@ -85,7 +85,6 @@ public enum LinkType {
 	protected int length = 0;
 	protected int type = 0;
 	protected byte[] bufferPayload = null;
-	protected int receiveLength = 0;
 
 	protected int frameWidth = -1;
 	protected int frameHeight = -1;
@@ -390,7 +389,6 @@ protected int receivePacket() {
 			csSerial = ((buffer[3] & 0xff) << 8) | (buffer[2] & 0xff);
 
 			res = link.receive(buffer, length, csCalc);
-			receiveLength = length;
 
 			if (res < 0)
 				return res;
@@ -405,7 +403,6 @@ protected int receivePacket() {
 			length = buffer[1];
 
 			res = link.receive(buffer, length);
-			receiveLength = length;
 
 			if (res < 0)
 				return res;

src/main/java/io/github/pseudoresonance/pixy2api/Pixy2CCC.java

@@ -94,7 +94,7 @@ public int getBlocks(boolean wait, int sigmap, int maxBlocks) {
 			if (pixy.receivePacket() == 0) {
 				if (pixy.type == CCC_RESPONSE_BLOCKS) {
 					blocks = new ArrayList<Block>();
-					for (int i = 0; i + 13 < pixy.receiveLength; i += 14) {
+					for (int i = 0; i + 13 < pixy.length; i += 14) {
 						Block b = new Block(((pixy.buffer[i + 1] & 0xff) << 8) | (pixy.buffer[i] & 0xff),
 								((pixy.buffer[i + 3] & 0xff) << 8) | (pixy.buffer[i + 2] & 0xff),
 								((pixy.buffer[i + 5] & 0xff) << 8) | (pixy.buffer[i + 4] & 0xff),

src/main/java/io/github/pseudoresonance/pixy2api/Pixy2Line.java

@@ -104,6 +104,8 @@ public byte getFeatures(byte type, byte features, boolean wait) {
 		vectors = null;
 		intersections = null;
 		barcodes = null;
+		
+		long start = System.currentTimeMillis();
 
 		while (true) {
 			// fill in request data
@@ -120,10 +122,10 @@ public byte getFeatures(byte type, byte features, boolean wait) {
 					for (offset = 0, res = 0; pixy.length > offset; offset += fsize + 2) {
 						ftype = pixy.buffer[offset];
 						fsize = pixy.buffer[offset + 1];
-						fdata = Arrays.copyOfRange(pixy.buffer, offset + 2, pixy.receiveLength);
+						fdata = Arrays.copyOfRange(pixy.buffer, offset + 2, pixy.length);
 						if (ftype == LINE_VECTOR) {
 							vectors = new Vector[(int) Math.floor(fdata.length / 6)];
-							for (int i = 0; (i + 1) * 6 < fdata.length; i++) {
+							for (int i = 0; (i + 1) * 6 <= fdata.length; i++) {
 								vectors[i] = new Vector(fdata[(6 * i)] & 0xFF, fdata[(6 * i) + 1] & 0xFF,
 										fdata[(6 * i) + 2] & 0xFF, fdata[(6 * i) + 3] & 0xFF, fdata[(6 * i) + 4] & 0xFF,
 										fdata[(6 * i) + 5] & 0xFF);
@@ -135,12 +137,13 @@ public byte getFeatures(byte type, byte features, boolean wait) {
 									.floor(fdata.length / (4 + (4 * LINE_MAX_INTERSECTION_LINES)))];
 							for (int i = 0; (i + 1) * size < fdata.length; i++) {
 								IntersectionLine[] lines = new IntersectionLine[LINE_MAX_INTERSECTION_LINES];
-								for (int l = 0; l < LINE_MAX_INTERSECTION_LINES; l++) {
+								for (int l = 0; l <= LINE_MAX_INTERSECTION_LINES; l++) {
 									int arr = ((size * i) + 4);
 									int index = fdata[arr + (4 * l)];
 									int reserved = fdata[arr + (4 * l) + 1];
-									short angle = (short) (((fdata[arr + (4 * l) + 2] & 0xff) << 8)
-											| (fdata[arr + (4 * l) + 3] & 0xff));
+									short angle = (short) (
+											((fdata[arr + (4 * l) + 3] & 0xff) << 8)
+											| (fdata[arr + (4 * l) + 2] & 0xff));
 									IntersectionLine intLine = new IntersectionLine(index, reserved, angle);
 									lines[l] = intLine;
 								}
@@ -151,7 +154,7 @@ public byte getFeatures(byte type, byte features, boolean wait) {
 							res |= LINE_INTERSECTION;
 						} else if (ftype == LINE_BARCODE) {
 							barcodes = new Barcode[(int) Math.floor(fdata.length / 4)];
-							for (int i = 0; (i + 1) * 4 < fdata.length; i++) {
+							for (int i = 0; (i + 1) * 4 <= fdata.length; i++) {
 								barcodes[i] = new Barcode(fdata[(4 * i)] & 0xFF, fdata[(4 * i) + 1] & 0xFF,
 										fdata[(4 * i) + 2] & 0xFF, fdata[(4 * i) + 3] & 0xFF);
 							}
@@ -170,6 +173,9 @@ else if (!wait) // we're busy
 			} else
 				return Pixy2.PIXY_RESULT_ERROR; // some kind of bitstream error
 
+			if (System.currentTimeMillis() - start > 500) {
+				return Pixy2.PIXY_RESULT_ERROR; // timeout to prevent lockup
+			}
 			// If we're waiting for frame data, don't thrash Pixy with requests.
 			// We can give up half a millisecond of latency (worst case)
 			try {