Add support for new Castel format

2 files changed, 26 insertions, 3 deletions

diff --git a/src/org/traccar/protocol/CastelProtocolDecoder.java b/src/org/traccar/protocol/CastelProtocolDecoder.java
index e36d086f8..4b8f3b716 100644
--- a/src/org/traccar/protocol/CastelProtocolDecoder.java
+++ b/src/org/traccar/protocol/CastelProtocolDecoder.java
@@ -115,9 +115,14 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
         ChannelBuffer buf = (ChannelBuffer) msg;
 
-        buf.skipBytes(2); // header
+        int header = buf.readUnsignedShort();
         buf.readUnsignedShort(); // length
-        int version = buf.readUnsignedByte();
+
+        int version = -1;
+        if (header == 0x4040) {
+            version = buf.readUnsignedByte();
+        }
+
         ChannelBuffer id = buf.readBytes(20);
         int type = ChannelBuffers.swapShort(buf.readShort());
 
@@ -125,7 +130,19 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
             return null;
         }
 
-        if (version == 4) {
+        if (version == -1) {
+
+            if (type == 0x2001) {
+
+                buf.readUnsignedInt(); // index
+                buf.readUnsignedInt(); // unix time
+                buf.readUnsignedByte();
+
+                return readPosition(buf);
+
+            }
+
+        } else if (version == 4) {
 
             if (type == MSG_SC_HEARTBEAT) {
 
diff --git a/test/org/traccar/protocol/CastelProtocolDecoderTest.java b/test/org/traccar/protocol/CastelProtocolDecoderTest.java
index b8795e09a..eafe80748 100644
--- a/test/org/traccar/protocol/CastelProtocolDecoderTest.java
+++ b/test/org/traccar/protocol/CastelProtocolDecoderTest.java
@@ -12,6 +12,12 @@ public class CastelProtocolDecoderTest extends ProtocolTest {
 
         CastelProtocolDecoder decoder = new CastelProtocolDecoder(new CastelProtocol());
 
+        verifyPosition(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+                "24243f00676e6768656636313031313132393030313734002001840d0000d2deb556020602100b35360456cf09e6ebac0200000000030000000001abc10d0a"));
+
+        verifyPosition(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+                "24243f00676e6768656636313031313132393030313734002001840d000000dfb556020602100b36298256cf0956ebac020000990c7f0000000001b4830d0a"));
+
         verifyPositions(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040590004313030303030303030303800000000000000000040010072f53f56c25240560000000078b00900000000009c3100000000030100011900030001090b0f080106c04fe40b4037310c0060e001ff018d01e05e0d0a"));