DEV-574 #time 1h better byte throwing handling and timestamp fail-safe

Author: marknolan

ShimmerDriver/src/main/java/com/shimmerresearch/bluetooth/ShimmerBluetooth.java

@@ -715,7 +715,7 @@ else if((mCurrentCommand==GET_FW_VERSION_COMMAND)
 					}
 					threadSleep((int)((Math.random()+.1)*100.0));
 					writeBytes(insBytes);
-					printLogDataForDebugging("Command Transmitted: \t\t\t" + btCommandToString(mCurrentCommand) + " " + UtilShimmer.bytesToHexStringWithSpacesFormatted(insBytes));
+					printLogDataForDebugging("Command Transmitted: \t\t\t" + btCommandToString(mCurrentCommand) + ", len=" + insBytes.length + " " + UtilShimmer.bytesToHexStringWithSpacesFormatted(insBytes));
 
 					//TODO: are the two stops needed here? better to wait for ack from Shimmer
 					if(mCurrentCommand==STOP_STREAMING_COMMAND 
@@ -880,7 +880,7 @@ private void processNotStreamingWaitForResp() {
 						mWaitForResponse=false;
 						mTransactionCompleted=true;
 						setInstructionStackLock(false);
-						printLogDataForDebugging("Response Received:\t\t\t" + btCommandToString(responseCommand));
+						printLogDataForDebugging("Response Received:\t\t\t" + btCommandToString(responseCommand) + ", len=" + byteBuffer.length + " " + UtilShimmer.bytesToHexStringWithSpacesFormatted(byteBuffer));
 
 						// Special case for FW_VERSION_RESPONSE because it
 						// needs to initialize the Shimmer after releasing
@@ -939,7 +939,7 @@ protected void processPacket() {
 				&& bufferTemp[getPacketSizeWithCrc()+1]==DATA_PACKET){
 
 			if (mBtCommsCrcModeCurrent != BT_CRC_MODE.OFF && !checkCrc(bufferTemp, getPacketSize() + 1)) {
-				discardFirstBufferByte();
+				discardBufferBytesToNextPacket();
 				return;
 			}
 
@@ -1002,8 +1002,8 @@ else if(isSupportedInStreamCmds() && bufferTemp[getPacketSizeWithCrc()+2]==INSTR
 				}
 			} 
 			if(mByteArrayOutputStream.size()>getPacketSizeWithCrc()+2){
-				printLogDataForDebugging("Unknown packet error (check with JC):\tExpected: " + (getPacketSizeWithCrc()+2) + "bytes but buffer contains " + mByteArrayOutputStream.size() + "bytes");
-				discardFirstBufferByte(); //throw the first byte away
+				printLogDataForDebugging("Unknown packet error: \tExpected: " + (getPacketSizeWithCrc()+2) + "bytes but buffer contains " + mByteArrayOutputStream.size() + "bytes");
+				discardBufferBytesToNextPacket(); // Skip to start of next packet
 			}
 
 		} 
@@ -1012,7 +1012,7 @@ else if(isSupportedInStreamCmds() && bufferTemp[getPacketSizeWithCrc()+2]==INSTR
 		else {
 			printLogDataForDebugging("Packet syncing problem:\tExpected: " + (getPacketSizeWithCrc()+2) + "bytes. Buffer contains " + mByteArrayOutputStream.size() + "bytes" 
 									+ "\nBuffer = " + UtilShimmer.bytesToHexStringWithSpacesFormatted(mByteArrayOutputStream.toByteArray()));
-			discardFirstBufferByte(); //throw the first byte away
+			discardBufferBytesToNextPacket(); // Skip to start of next packet
 		}
 	}
 
@@ -1052,7 +1052,7 @@ public boolean checkCrc(byte[] bufferTemp, int length) {
 		if (mBtCommsCrcModeCurrent == BT_CRC_MODE.TWO_BYTE_CRC) {
 			// + 2 as this is the location of the CRC's MSB
 			if (bufferTemp[getPacketSize() + 2] != crcCalc[1]) {
-				discardFirstBufferByte();
+				discardBufferBytesToNextPacket();
 				return false;
 			}
 		}
@@ -1190,6 +1190,10 @@ private void buildAndSendMsg(byte[] packet, COMMUNICATION_TYPE fwType, boolean t
 		ObjectCluster objectCluster = null;
 		try {
 			objectCluster = buildMsg(packet, fwType, timeSync, pcTimeStamp);
+			if(objectCluster.successfullyParsed==false){
+				printLogDataForDebugging("Packet timestamp is not contiguous - skipping:\t\t" + UtilShimmer.bytesToHexStringWithSpacesFormatted(packet));
+				return;
+			}
 			objectCluster = systemTimestampPlot.processSystemTimestampPlot(objectCluster);
 		} catch (Exception e) {
 			e.printStackTrace();
@@ -1294,16 +1298,34 @@ protected void clearBuffers() {
 	}
 
 	/**
-	 * 
+	 * Next packet start should begin with DATA_PACKET or ACK_COMMAND_PROCESSED byte so skip to that point
 	 */
-	protected void discardFirstBufferByte(){
+	protected void discardBufferBytesToNextPacket(){
 		byte[] bTemp = mByteArrayOutputStream.toByteArray();
+		
+		//Find index of first DATA_PACKET or ACK byte within the buffer
+		int offset = findOffsetOfNextZeroOrFF(bTemp);
+		//If not found, just skip one byte
+		offset = (offset == -1) ? 1 : offset;
+		
 		mByteArrayOutputStream.reset();
-		mByteArrayOutputStream.write(bTemp, 1, bTemp.length-1); //this will throw the first byte away
+		mByteArrayOutputStream.write(bTemp, offset, bTemp.length-offset); //this will throw the first byte away
 		if(mEnablePCTimeStamps) {
-			mListofPCTimeStamps.remove(0);
+			for (int i=0;i<offset;i++)
+			{
+				mListofPCTimeStamps.remove(0);
+			}
+		}
+		consolePrintLn("Throw Bytes " + UtilShimmer.bytesToHexStringWithSpacesFormatted(Arrays.copyOfRange(bTemp, 0, offset)));
+	}
+	
+	private static int findOffsetOfNextZeroOrFF(byte[] a) {
+		for (int i = 1; i < a.length; i++) {
+			byte b = a[i];
+			if (b == 0 || b == (byte) 0xFF)
+				return i;
 		}
-		consolePrintLn("Throw Byte" + UtilShimmer.bytesToHexStringWithSpacesFormatted(bTemp));
+		return -1;
 	}
 
 	/**

ShimmerDriver/src/main/java/com/shimmerresearch/driver/ObjectCluster.java

@@ -156,6 +156,8 @@ public enum OBJECTCLUSTER_TYPE{
 	public int mIndexCal = 0;
 	public int mIndexUncal = 0;
 	public boolean mEnableArraysDataStructure = false;
+
+	public boolean successfullyParsed = true;
 
 	public class SensorDataPerType {
 

ShimmerDriver/src/main/java/com/shimmerresearch/driver/ShimmerObject.java

@@ -494,6 +494,8 @@ public class BTStream {
 	public static final int MAX_NUMBER_OF_SIGNALS = 77;//50; //used to be 11 but now 13 because of the SR30 + 8 for 3d orientation
 	public static final int MAX_INQUIRY_PACKET_SIZE = 47;
 
+	public static final int CONTIGUOUS_TIMESTAMP_TICKS_LIMIT = (10*32768); // 10 seconds worth of ticks
+	
 	public enum TEST_MODE {
 	    MAIN_TEST((byte)0, "Main Test"),
 	    LED_TEST((byte)1, "LED Test"),
@@ -786,6 +788,8 @@ public String toString() {
 
 	// ---------- GSR end ------------------
 
+	private double mLastReceivedTimeStampTicks = 0;
+
 	public ObjectCluster setLSLTimeIfAvailable(ObjectCluster ojc) {
 		return ojc;
 	}
@@ -923,7 +927,24 @@ public ObjectCluster buildMsg(byte[] newPacket, COMMUNICATION_TYPE fwType, boole
 
 		if (getHardwareVersion()==HW_ID.SHIMMER_SR30 || getHardwareVersion()==HW_ID.SHIMMER_3  || getHardwareVersion()==HW_ID.SHIMMER_3R  
 				|| getHardwareVersion()==HW_ID.SHIMMER_GQ_802154_LR || getHardwareVersion()==HW_ID.SHIMMER_GQ_802154_NR || getHardwareVersion()==HW_ID.SHIMMER_2R_GQ){
-			
+
+			/*
+			 * Even with checking for start and stop of packets while parsing and performing
+			 * CRC checks, some corrupted packets can still slip through. This is a final
+			 * check help to make sure the time stamp ticks are contiguous before parsing the
+			 * packet.
+			 */
+			if (fwType == COMMUNICATION_TYPE.BLUETOOTH)
+			{
+				double shimmerTimestampTicks = (double)newPacketInt[getSignalIndex(Configuration.Shimmer3.ObjectClusterSensorName.TIMESTAMP)];
+				if(mLastReceivedTimeStampTicks!=0 && Math.abs(shimmerTimestampTicks - mLastReceivedTimeStampTicks) > CONTIGUOUS_TIMESTAMP_TICKS_LIMIT) {
+					mLastReceivedTimeStampTicks = shimmerTimestampTicks;
+					objectCluster.successfullyParsed = false;
+					return objectCluster; //discard packet
+				}
+				mLastReceivedTimeStampTicks = shimmerTimestampTicks;
+			}
+
 			parseTimestampShimmer3(fwType, objectCluster, uncalibratedData, uncalibratedDataUnits, calibratedData, calibratedDataUnits, sensorNames, newPacketInt);
 
 			//OFFSET