Support Teltonika GH3000 protocol

3 files changed, 105 insertions, 30 deletions

diff --git a/src/org/traccar/model/ExtendedInfoFormatter.java b/src/org/traccar/model/ExtendedInfoFormatter.java
index e17c96cb8..756845b9d 100644
--- a/src/org/traccar/model/ExtendedInfoFormatter.java
+++ b/src/org/traccar/model/ExtendedInfoFormatter.java
@@ -40,8 +40,7 @@ public class ExtendedInfoFormatter {
 
     @Override
     public String toString() {
-        data.append("</").append(rootNode).append(">");
-        return data.toString();
+        return data.toString() + "</" + rootNode + ">";
     }
 
 }
diff --git a/src/org/traccar/protocol/TeltonikaProtocolDecoder.java b/src/org/traccar/protocol/TeltonikaProtocolDecoder.java
index 7561e35e1..2cc7d1477 100644
--- a/src/org/traccar/protocol/TeltonikaProtocolDecoder.java
+++ b/src/org/traccar/protocol/TeltonikaProtocolDecoder.java
@@ -55,13 +55,21 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
             channel.write(response);
         }
     }
+
+    private static boolean checkBit(long mask, int bit) {
+        long checkMask = 1 << bit;
+        return (mask & checkMask) == checkMask;
+    }
+
+    private static final int CODEC_GH3000 = 0x07;
+    private static final int CODEC_FM4X00 = 0x08;
     
     private List<Position> parseLocation(Channel channel, ChannelBuffer buf) {
         List<Position> positions = new LinkedList<Position>();
         
         buf.skipBytes(4); // marker
         buf.readUnsignedInt(); // data length
-        buf.readUnsignedByte(); // codec
+        int codec = buf.readUnsignedByte(); // codec
         int count = buf.readUnsignedByte();
         
         for (int i = 0; i < count; i++) {
@@ -69,48 +77,113 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
             ExtendedInfoFormatter extendedInfo = new ExtendedInfoFormatter("teltonika");
             
             position.setDeviceId(deviceId);
-            position.setTime(new Date(buf.readLong()));
+            
+            int globalMask = 0x0f;
+            
+            if (codec == CODEC_GH3000) {
 
-            extendedInfo.set("priority", buf.readUnsignedByte());
+                long time = buf.readUnsignedInt() & 0x3fffffff;
+                time += 1199145600; // 2008-01-01 00:00:00
+                position.setTime(new Date(time * 1000));
+                
+                globalMask = buf.readUnsignedByte();
+                if (!checkBit(globalMask, 0)) {
+                    return null;
+                }
+                
+                int locationMask = buf.readUnsignedByte();
+                
+                if (checkBit(locationMask, 0)) {
+                    position.setLatitude(Double.valueOf(buf.readFloat()));
+                    position.setLongitude(Double.valueOf(buf.readFloat()));
+                }
+                
+                if (checkBit(locationMask, 1)) {
+                    position.setAltitude((double) buf.readUnsignedShort());
+                }
+                
+                if (checkBit(locationMask, 2)) {
+                    position.setCourse(buf.readUnsignedByte() * 360.0 / 256);
+                }
+                
+                if (checkBit(locationMask, 3)) {
+                    position.setSpeed((double) buf.readUnsignedByte());
+                }
+                
+                if (checkBit(locationMask, 4)) {
+                    int satellites = buf.readUnsignedByte();
+                    extendedInfo.set("satellites", satellites);
+                    position.setValid(satellites >= 3);
+                }
+                
+                if (checkBit(locationMask, 5)) {
+                    extendedInfo.set("area", buf.readUnsignedShort());
+                    extendedInfo.set("cell", buf.readUnsignedShort());
+                }
+                
+                if (checkBit(locationMask, 6)) {
+                    extendedInfo.set("gsm", buf.readUnsignedByte());
+                }
+                
+                if (checkBit(locationMask, 7)) {
+                    extendedInfo.set("operator", buf.readUnsignedInt());
+                }
 
-            position.setLongitude(buf.readInt() / 10000000.0);
-            position.setLatitude(buf.readInt() / 10000000.0);
-            position.setAltitude((double) buf.readUnsignedShort());
-            position.setCourse((double) buf.readUnsignedShort());
-            
-            int satellites = buf.readUnsignedByte();
-            extendedInfo.set("satellites", satellites);
+            } else {
 
-            position.setValid(satellites != 0);
-            
-            position.setSpeed(buf.readUnsignedShort() * 0.539957);
+                position.setTime(new Date(buf.readLong()));
 
-            extendedInfo.set("event", buf.readUnsignedByte());
-            
-            buf.readUnsignedByte(); // total IO data records
+                extendedInfo.set("priority", buf.readUnsignedByte());
+
+                position.setLongitude(buf.readInt() / 10000000.0);
+                position.setLatitude(buf.readInt() / 10000000.0);
+                position.setAltitude((double) buf.readUnsignedShort());
+                position.setCourse((double) buf.readUnsignedShort());
 
+                int satellites = buf.readUnsignedByte();
+                extendedInfo.set("satellites", satellites);
+
+                position.setValid(satellites != 0);
+
+                position.setSpeed(buf.readUnsignedShort() * 0.539957);
+
+                extendedInfo.set("event", buf.readUnsignedByte());
+
+                buf.readUnsignedByte(); // total IO data records
+
+            }
+            
             // Read 1 byte data
-            int cnt = buf.readUnsignedByte();
-            for (int j = 0; j < cnt; j++) {
-                extendedInfo.set("io" + buf.readUnsignedByte(), buf.readUnsignedByte());
+            if (checkBit(globalMask, 1)) {
+                int cnt = buf.readUnsignedByte();
+                for (int j = 0; j < cnt; j++) {
+                    extendedInfo.set("io" + buf.readUnsignedByte(), buf.readUnsignedByte());
+                }
             }
+
             
             // Read 2 byte data
-            cnt = buf.readUnsignedByte();
-            for (int j = 0; j < cnt; j++) {
-                extendedInfo.set("io" + buf.readUnsignedByte(), buf.readUnsignedShort());
+            if (checkBit(globalMask, 2)) {
+                int cnt = buf.readUnsignedByte();
+                for (int j = 0; j < cnt; j++) {
+                    extendedInfo.set("io" + buf.readUnsignedByte(), buf.readUnsignedShort());
+                }
             }
 
             // Read 4 byte data
-            cnt = buf.readUnsignedByte();
-            for (int j = 0; j < cnt; j++) {
-                extendedInfo.set("io" + buf.readUnsignedByte(), buf.readUnsignedInt());
+            if (checkBit(globalMask, 3)) {
+                int cnt = buf.readUnsignedByte();
+                for (int j = 0; j < cnt; j++) {
+                    extendedInfo.set("io" + buf.readUnsignedByte(), buf.readUnsignedInt());
+                }
             }
 
             // Read 8 byte data
-            cnt = buf.readUnsignedByte();
-            for (int j = 0; j < cnt; j++) {
-                extendedInfo.set("io" + buf.readUnsignedByte(), buf.readLong());
+            if (codec == CODEC_FM4X00) {
+                int cnt = buf.readUnsignedByte();
+                for (int j = 0; j < cnt; j++) {
+                    extendedInfo.set("io" + buf.readUnsignedByte(), buf.readLong());
+                }
             }
         
             position.setExtendedInfo(extendedInfo.toString());
diff --git a/test/org/traccar/protocol/TeltonikaProtocolDecoderTest.java b/test/org/traccar/protocol/TeltonikaProtocolDecoderTest.java
index 1b8ad378d..789844f50 100644
--- a/test/org/traccar/protocol/TeltonikaProtocolDecoderTest.java
+++ b/test/org/traccar/protocol/TeltonikaProtocolDecoderTest.java
@@ -25,6 +25,9 @@ public class TeltonikaProtocolDecoderTest {
         byte[] buf4 = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x08,0x01,0x00,0x00,0x01,0x3e,(byte)0xff,(byte)0x8d,0x6f,(byte)0x98,0x00,0x17,0x32,(byte)0x95,0x00,0x21,0x11,(byte)0xf4,0x00,0x00,(byte)0x81,0x00,(byte)0xae,0x0b,0x00,0x00,0x00,0x04,0x01,0x01,0x00,0x03,0x09,0x00,0x16,0x43,0x29,(byte)0x80,0x42,0x2f,0x72,0x00,0x00,0x01,0x00,0x00,0x7a,0x5d};
         assertNotNull(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(buf4)));
         
+        byte[] buf5 = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,(byte)0xc7,0x07,0x04,0x41,(byte)0xbf,(byte)0x9d,(byte)0xb0,0x0f,(byte)0xff,0x42,0x5a,(byte)0xdb,(byte)0xd7,0x41,(byte)0xca,0x6e,0x1e,0x00,(byte)0x9e,0x12,0x05,0x07,0x00,0x01,0x03,0x0b,0x16,0x00,0x00,0x60,0x1a,0x02,0x01,0x5e,0x02,0x00,0x03,0x14,0x00,0x66,0x15,0x00,0x0a,0x16,0x00,0x67,0x01,0x05,0x00,0x00,0x0c,(byte)0xe4,0x41,(byte)0xbf,(byte)0x9d,(byte)0x92,0x0f,(byte)0xff,0x42,0x5a,(byte)0xdb,(byte)0xb1,0x41,(byte)0xca,0x6f,(byte)0xc9,0x00,(byte)0xa2,(byte)0xb2,0x18,0x07,0x00,0x01,0x03,0x0b,0x16,0x00,0x00,0x60,0x1a,0x02,0x01,0x5e,0x02,0x00,0x03,0x14,0x00,0x66,0x15,0x00,0x0a,0x16,0x00,0x67,0x01,0x05,0x00,0x00,0x0c,(byte)0xc6,0x41,(byte)0xbf,(byte)0x9d,0x74,0x0f,(byte)0xff,0x42,0x5a,(byte)0xdb,(byte)0xee,0x41,(byte)0xca,0x73,(byte)0x92,0x00,(byte)0xb6,(byte)0xc9,0x1e,0x07,0x00,0x01,0x03,0x0b,0x1f,0x00,0x00,0x60,0x1a,0x02,0x01,0x5f,0x02,0x00,0x03,0x14,0x00,0x66,0x15,0x00,0x0a,0x16,0x00,0x66,0x01,0x05,0x00,0x00,0x0c,(byte)0xa8,0x41,(byte)0xbf,(byte)0x9c,(byte)0xfc,0x0f,(byte)0xff,0x42,0x5a,(byte)0xdb,(byte)0xa0,0x41,(byte)0xca,0x70,(byte)0xc1,0x00,(byte)0xb9,0x38,0x13,0x07,0x00,0x01,0x03,0x0b,0x1f,0x00,0x00,0x60,0x1a,0x02,0x01,0x5f,0x02,0x00,0x03,0x14,0x00,0x23,0x15,0x00,0x0a,0x16,0x00,0x25,0x01,0x05,0x00,0x00,0x0c,0x30,0x04,0x00,0x00,0x00};
+        assertNotNull(decoder.decode(null, null, ChannelBuffers.wrappedBuffer(buf5)));
+        
     }
 
 }