updated innovate plugins

git-svn-id: https://svn2.assembla.com/svn/romraider/trunk@28 38686702-15cf-42e4-a595-3071df8bf5ea

src/enginuity/logger/innovate/generic/io/InnovateConnectionImpl.java

@@ -4,15 +4,22 @@ import enginuity.io.connection.ConnectionProperties;
 import enginuity.io.connection.SerialConnection;
 import enginuity.io.connection.SerialConnectionImpl;
 import enginuity.logger.ecu.exception.SerialCommunicationException;
-import static enginuity.util.HexUtil.asHex;
+import static enginuity.util.ByteUtil.matchOnes;
+import static enginuity.util.ByteUtil.matchZeroes;
+import static enginuity.util.HexUtil.asBytes;
 import static enginuity.util.ParamChecker.checkGreaterThanZero;
 import static enginuity.util.ParamChecker.checkNotNull;
 import static enginuity.util.ParamChecker.checkNotNullOrEmpty;
 import static enginuity.util.ThreadUtil.sleep;
 import org.apache.log4j.Logger;
+import java.io.IOException;
+import static java.lang.Math.min;
+import static java.lang.System.arraycopy;
+import static java.lang.System.currentTimeMillis;
 
 public final class InnovateConnectionImpl implements InnovateConnection {
     private static final Logger LOGGER = Logger.getLogger(InnovateConnectionImpl.class);
+    private static final byte[] INNOVATE_HEADER = asBytes("0xB280");
     private final String device;
     private final long sendTimeout;
     private final SerialConnection serialConnection;
@@ -30,30 +37,73 @@ public final class InnovateConnectionImpl implements InnovateConnection {
         LOGGER.info(device + " connected");
     }
 
+    // FIX - YIKES!!
     public byte[] read() {
         try {
             byte[] response = new byte[responseLength];
-            serialConnection.readStaleData();
-            long start = System.currentTimeMillis();
-            while (serialConnection.available() < response.length) {
-                if (System.currentTimeMillis() - start > sendTimeout) {
-                    byte[] badBytes = serialConnection.readAvailable();
-                    LOGGER.warn(device + " Response [read timeout]: " + asHex(badBytes));
-                    return badBytes;
+            int bufferLength = responseLength + INNOVATE_HEADER.length - 1;
+            long start = currentTimeMillis();
+            while (currentTimeMillis() - start <= sendTimeout) {
+                sleep(1);
+                int available = serialConnection.available();
+                if (available < bufferLength) continue;
+                byte[] buffer = new byte[bufferLength];
+                serialConnection.read(buffer);
+                int responseBeginIndex = 0;
+                int bufferBeginIndex = findHeader(buffer);
+                if (bufferBeginIndex < 0) {
+                    bufferBeginIndex = findLm1(buffer);
+                    if (bufferBeginIndex < 0) continue;
+                    arraycopy(INNOVATE_HEADER, 0, response, 0, INNOVATE_HEADER.length);
+                    responseBeginIndex = INNOVATE_HEADER.length;
                 }
-                sleep(5);
+                int tailLength = responseLength - responseBeginIndex;
+                arraycopy(buffer, bufferBeginIndex, response, responseBeginIndex, min(tailLength, (buffer.length - bufferBeginIndex)));
+                int remainderLength = tailLength - (buffer.length - bufferBeginIndex);
+                if (remainderLength > 0) {
+                    byte[] remainder = remainder(remainderLength, start);
+                    if (remainder.length == 0) continue;
+                    arraycopy(remainder, 0, response, responseLength - remainderLength, remainderLength);
+                }
+                return response;
             }
-            serialConnection.read(response);
-            LOGGER.trace(device + " Response: " + asHex(response));
-            return response;
+            LOGGER.warn(device + " Response [read timeout]");
+            return new byte[0];
         } catch (Exception e) {
             close();
             throw new SerialCommunicationException(e);
         }
     }
 
+    private byte[] remainder(int remainderLength, long start) throws IOException {
+        while (currentTimeMillis() - start <= sendTimeout) {
+            sleep(1);
+            int available = serialConnection.available();
+            if (available >= remainderLength) {
+                byte[] remainder = new byte[remainderLength];
+                serialConnection.read(remainder);
+                return remainder;
+            }
+        }
+        return new byte[0];
+    }
+
     public void close() {
         serialConnection.close();
         LOGGER.info(device + " disconnected");
     }
+
+    private int findHeader(byte[] bytes) {
+        for (int i = 0; i < bytes.length - 1; i++) {
+            if (matchOnes(bytes[i], 178) && matchOnes(bytes[i + 1], 128)) return i;
+        }
+        return -1;
+    }
+
+    private int findLm1(byte[] bytes) {
+        for (int i = 0; i < bytes.length; i++) {
+            if (matchOnes(bytes[i], 128) && matchZeroes(bytes[i], 34)) return i;
+        }
+        return -1;
+    }
 }

src/enginuity/logger/innovate/lc1/plugin/Lc1DataConvertor.java

@@ -1,6 +1,8 @@
 package enginuity.logger.innovate.lc1.plugin;
 
 import enginuity.logger.innovate.generic.plugin.DataConvertor;
+import static enginuity.util.ByteUtil.matchOnes;
+import static enginuity.util.ByteUtil.matchZeroes;
 import static enginuity.util.HexUtil.asHex;
 import org.apache.log4j.Logger;
 
@@ -64,12 +66,4 @@ public final class Lc1DataConvertor implements DataConvertor {
     private int getLambda(byte[] bytes) {
         return ((bytes[4] & 63) << 7) | bytes[5];
     }
-
-    private boolean matchOnes(int b, int mask) {
-        return (b & mask) == mask;
-    }
-
-    private boolean matchZeroes(int b, int mask) {
-        return (b & mask) == 0;
-    }
 }

src/enginuity/logger/innovate/lm1/plugin/Lm1DataConvertor.java

@@ -1,6 +1,9 @@
 package enginuity.logger.innovate.lm1.plugin;
 
 import enginuity.logger.innovate.generic.plugin.DataConvertor;
+import static enginuity.util.ByteUtil.matchOnes;
+import static enginuity.util.ByteUtil.matchZeroes;
+import static enginuity.util.HexUtil.asBytes;
 import static enginuity.util.HexUtil.asHex;
 import org.apache.log4j.Logger;
 
@@ -63,11 +66,10 @@ public final class Lm1DataConvertor implements DataConvertor {
         return (bytes[4] << 7) | bytes[5];
     }
 
-    private boolean matchOnes(int b, int mask) {
-        return (b & mask) == mask;
-    }
-
-    private boolean matchZeroes(int b, int mask) {
-        return (b & mask) == 0;
+    public static void main(String[] args) {
+        byte[] bytes = asBytes("0xB2808113036F1E650124007000470039003A");
+        DataConvertor convertor = new Lm1DataConvertor();
+        double result = convertor.convert(bytes);
+        System.out.println("result = " + result);
     }
 }

src/enginuity/util/ByteUtil.java

@@ -22,7 +22,6 @@
 package enginuity.util;
 
 import enginuity.newmaps.ecumetadata.Scale;
-
 import java.nio.ByteBuffer;
 
 @SuppressWarnings({"UnnecessaryBoxing"})
@@ -121,7 +120,7 @@ public final class ByteUtil {
     }
 
     public static float asFloat(byte[] bytes, int endian) {
-        byte[] currentCell = { bytes[0],bytes[1],bytes[2],bytes[3] };
+        byte[] currentCell = {bytes[0], bytes[1], bytes[2], bytes[3]};
         return Float.intBitsToFloat(asUnsignedInt(currentCell, endian));
     }
 
@@ -132,4 +131,12 @@ public final class ByteUtil {
         return bb.array();
     }
 
+    public static boolean matchOnes(byte b, int mask) {
+        return (b & mask) == mask;
+    }
+
+    public static boolean matchZeroes(byte b, int mask) {
+        return (b & mask) == 0;
+    }
+
 }