Implement SinoCastel OBD decoding

3 files changed, 131 insertions, 38 deletions

diff --git a/src/org/traccar/helper/ObdDecoder.java b/src/org/traccar/helper/ObdDecoder.java
index 6ffe39662..8383a2b1a 100644
--- a/src/org/traccar/helper/ObdDecoder.java
+++ b/src/org/traccar/helper/ObdDecoder.java
@@ -42,7 +42,9 @@ public final class ObdDecoder {
         switch (mode) {
             case MODE_CURRENT:
             case MODE_FREEZE_FRAME:
-                return decodeData(Integer.parseInt(value.substring(0, 2), 16), value.substring(2));
+                return decodeData(
+                        Integer.parseInt(value.substring(0, 2), 16),
+                        Integer.parseInt(value.substring(2), 16));
             case MODE_CODES:
                 return decodeCodes(value);
             default:
@@ -82,25 +84,24 @@ public final class ObdDecoder {
         }
     }
 
-    private static Map.Entry<String, Object> decodeData(int pid, String value) {
-        int intValue = Integer.parseInt(value, 16);
+    public static Map.Entry<String, Object> decodeData(int pid, int value) {
         switch (pid) {
             case PID_ENGINE_LOAD:
-                return createEntry("engineLoad", intValue * 100 / 255);
+                return createEntry("engineLoad", value * 100 / 255);
             case PID_COOLANT_TEMPERATURE:
-                return createEntry("coolantTemperature", intValue - 40);
+                return createEntry("coolantTemperature", value - 40);
             case PID_ENGINE_RPM:
-                return createEntry(Position.KEY_RPM, intValue / 4);
+                return createEntry(Position.KEY_RPM, value / 4);
             case PID_VEHICLE_SPEED:
-                return createEntry(Position.KEY_OBD_SPEED, intValue);
+                return createEntry(Position.KEY_OBD_SPEED, value);
             case PID_THROTTLE_POSITION:
-                return createEntry("throttle", intValue * 100 / 255);
+                return createEntry("throttle", value * 100 / 255);
             case PID_MIL_DISTANCE:
-                return createEntry("milDistance", intValue);
+                return createEntry("milDistance", value);
             case PID_FUEL_LEVEL:
-                return createEntry(Position.KEY_FUEL, intValue * 100 / 255);
+                return createEntry(Position.KEY_FUEL, value * 100 / 255);
             case PID_DISTANCE_CLEARED:
-                return createEntry("clearedDistance", intValue);
+                return createEntry("clearedDistance", value);
             default:
                 return null;
         }
diff --git a/src/org/traccar/protocol/CastelProtocolDecoder.java b/src/org/traccar/protocol/CastelProtocolDecoder.java
index b8faed077..ee5fac5e6 100644
--- a/src/org/traccar/protocol/CastelProtocolDecoder.java
+++ b/src/org/traccar/protocol/CastelProtocolDecoder.java
@@ -29,11 +29,46 @@ import org.traccar.model.Position;
 import java.net.SocketAddress;
 import java.nio.ByteOrder;
 import java.nio.charset.StandardCharsets;
+import java.util.HashMap;
 import java.util.LinkedList;
 import java.util.List;
+import java.util.Map;
 
 public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
+    private static final Map<Integer, Integer> PID_LENGTH_MAP = new HashMap<>();
+
+    static {
+        int[] l1 = {
+            0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0b, 0x0d,
+            0x0e, 0x0f, 0x11, 0x12, 0x13, 0x1c, 0x1d, 0x1e, 0x2c,
+            0x2d, 0x2e, 0x2f, 0x30, 0x33, 0x43, 0x45, 0x46,
+            0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x51, 0x52,
+            0x5a
+        };
+        int[] l2 = {
+            0x02, 0x03, 0x0a, 0x0c, 0x10, 0x14, 0x15, 0x16,
+            0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1f, 0x21, 0x22,
+            0x23, 0x31, 0x32, 0x3c, 0x3d, 0x3e, 0x3f, 0x42,
+            0x44, 0x4d, 0x4e, 0x50, 0x53, 0x54, 0x55, 0x56,
+            0x57, 0x58, 0x59
+        };
+        int[] l4 = {
+            0x00, 0x01, 0x20, 0x24, 0x25, 0x26, 0x27, 0x28,
+            0x29, 0x2a, 0x2b, 0x34, 0x35, 0x36, 0x37, 0x38,
+            0x39, 0x3a, 0x3b, 0x40, 0x41, 0x4f
+        };
+        for (int i : l1) {
+            PID_LENGTH_MAP.put(i, 1);
+        }
+        for (int i : l2) {
+            PID_LENGTH_MAP.put(i, 2);
+        }
+        for (int i : l4) {
+            PID_LENGTH_MAP.put(i, 4);
+        }
+    }
+
     public CastelProtocolDecoder(CastelProtocol protocol) {
         super(protocol);
     }
@@ -91,6 +126,63 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
         return position;
     }
 
+    private Position createPosition(DeviceSession deviceSession) {
+
+        Position position = new Position();
+        position.setProtocol(getProtocolName());
+        position.setDeviceId(deviceSession.getDeviceId());
+
+        getLastLocation(position, null);
+
+        return position;
+    }
+
+    private void decodeObd(Position position, ChannelBuffer buf, boolean groups) {
+
+        int count = buf.readUnsignedByte();
+
+        int[] pids = new int[count];
+        for (int i = 0; i < count; i++) {
+            pids[i] = buf.readUnsignedShort() & 0xff;
+        }
+
+        if (groups) {
+            buf.readUnsignedByte(); // group count
+            buf.readUnsignedByte(); // group size
+        }
+
+        for (int i = 0; i < count; i++) {
+            int value;
+            switch (PID_LENGTH_MAP.get(pids[i])) {
+                case 1:
+                    value = buf.readUnsignedByte();
+                    break;
+                case 2:
+                    value = buf.readUnsignedShort();
+                    break;
+                case 4:
+                    value = buf.readInt();
+                    break;
+                default:
+                    value = 0;
+                    break;
+            }
+            position.add(ObdDecoder.decodeData(pids[i], value));
+        }
+    }
+
+    private void decodeStat(Position position, ChannelBuffer buf) {
+
+        buf.readUnsignedInt(); // ACC ON time
+        buf.readUnsignedInt(); // UTC time
+        position.set(Position.KEY_ODOMETER, buf.readUnsignedInt());
+        buf.readUnsignedInt(); // trip odometer
+        buf.readUnsignedInt(); // total fuel consumption
+        buf.readUnsignedShort(); // current fuel consumption
+        position.set(Position.KEY_STATUS, buf.readUnsignedInt());
+        buf.skipBytes(8);
+    }
+
     private void sendResponse(
             Channel channel, SocketAddress remoteAddress,
             int version, ChannelBuffer id, short type, ChannelBuffer content) {
@@ -200,15 +292,22 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
             return readPosition(deviceSession, buf);
 
-        } else if (type == MSG_SC_DTCS_PASSENGER) {
+        } else if (type == MSG_SC_PID_DATA) {
 
-            Position position = new Position();
-            position.setProtocol(getProtocolName());
-            position.setDeviceId(deviceSession.getDeviceId());
+            Position position = createPosition(deviceSession);
 
-            getLastLocation(position, null);
+            decodeStat(position, buf);
 
-            buf.skipBytes(6 * 4 + 2 + 8);
+            buf.readUnsignedShort(); // sample rate
+            decodeObd(position, buf, true);
+
+            return position;
+
+        } else if (type == MSG_SC_DTCS_PASSENGER) {
+
+            Position position = createPosition(deviceSession);
+
+            decodeStat(position, buf);
 
             buf.readUnsignedByte(); // flag
             position.add(ObdDecoder.decodeCodes(ChannelBuffers.hexDump(buf.readBytes(buf.readUnsignedByte()))));
@@ -217,27 +316,20 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
         } else if (type == MSG_SC_OBD_DATA) {
 
-            Position position = new Position();
-            position.setProtocol(getProtocolName());
-            position.setDeviceId(deviceSession.getDeviceId());
-
-            getLastLocation(position, null);
+            Position position = createPosition(deviceSession);
 
-            buf.skipBytes(6 * 4 + 2 + 8);
+            decodeStat(position, buf);
 
-            // decode data
+            buf.readUnsignedByte(); // flag
+            decodeObd(position, buf, false);
 
             return position;
 
         } else if (type == MSG_SC_CELL) {
 
-            Position position = new Position();
-            position.setProtocol(getProtocolName());
-            position.setDeviceId(deviceSession.getDeviceId());
-
-            getLastLocation(position, null);
+            Position position = createPosition(deviceSession);
 
-            buf.skipBytes(6 * 4 + 2 + 8);
+            decodeStat(position, buf);
 
             position.set(Position.KEY_LAC, buf.readUnsignedShort());
             position.set(Position.KEY_CID, buf.readUnsignedShort());
diff --git a/test/org/traccar/protocol/CastelProtocolDecoderTest.java b/test/org/traccar/protocol/CastelProtocolDecoderTest.java
index ccdd6f726..e2f2c26bf 100644
--- a/test/org/traccar/protocol/CastelProtocolDecoderTest.java
+++ b/test/org/traccar/protocol/CastelProtocolDecoderTest.java
@@ -12,16 +12,16 @@ public class CastelProtocolDecoderTest extends ProtocolTest {
 
         CastelProtocolDecoder decoder = new CastelProtocolDecoder(new CastelProtocol());
 
-        verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "40405700043231334e583230313630303131373700000000004002c458ce572159ce57a9e2020082030000500c00000f0000000400036401240e0403023c000505210c210d210f21102101075b14030121330269430d0a"));
 
         verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "40407800043231334e583230313630303131373700000000004004fa52ce574b53ce57cad1020041020000050c00000d0000000400036401240b0503001b042105210c210d210f211021112113211c211f212121232124212c212d213021312133213e2141214221452149214a214c214f215021384e0d0a"));
 
-        verifyNotNull(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040a600043231334e583230313630303131373700000000004005fa52ce575053ce57cad102006b020000050c00000d0000000400036401240b050300001b042105210c210d210f211021112113211c211f212121232124212c212d213021312133213e2141214221452149214a214c214f215021015bd604301f500600000653000000bc0bffff78250000ff2d98642401000f8080e038000f0f0000000000000077b10d0a"));
 
-        verifyNotNull(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "40404300043231334e583230313630303131373700000000004006fa52ce574e53ce57cad1020053020000050c00000d0000000400036401240b0503000000feec0d0a"));
 
         verifyPosition(decoder, binary(ByteOrder.LITTLE_ENDIAN,
@@ -48,16 +48,16 @@ public class CastelProtocolDecoderTest extends ProtocolTest {
         verifyPositions(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "40406000043130303131313235323939383700000000000000400705000000C1F0695249F469529C9111000000000069830000D80040000400036401014C04030001190A0D04201E1480D60488C5721800000000AF0101060F000F00EA1E0D0A"));
 
-        verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "404057000431303031313132353239393837000000000000004002C1F06952F0F169529C9111000000000069830000470000000400036401014C01030078000505210C210D210F21102101073BE8030064280AEB930D0A"));
 
         verifyPositions(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "40405900043130303131313235323939383700000000000000400101C1F06952E7F069529C9111000000000069830000070000000400036401014C00030001190A0D0412041480D60488C57218000000009F01E803ED9A0D0A"));
 
-        verifyNotNull(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040B9000431303031313132353239393837000000000000004005C1F069521BF169529C9111000000000069830000130000000400036401014C0003000022032104210521062107210C210D210E210F2110211121132115211C211F21212124212E212F2130213121322133213C214221432144214521472149214A214C214D214E210100643B6232E803003E64280A3C24FE00010E010F00D5805A483C640000000000010000E02E000000066400000500000000A7710D0A"));
 
-        verifyNotNull(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "404043000431303031313132353239393837000000000000004006C1F0695209F169529C91110000000000698300000D0000000400036401014C00030000009AF40D0A"));
 
         verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,
@@ -72,7 +72,7 @@ public class CastelProtocolDecoderTest extends ProtocolTest {
         verifyPosition(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "40405c000c363131313530303030393536000000000000000040011c0a0f0e362dca53cd0860831303000000000300000000ff000000000000007ba083a650542d3639305f56312e312e320050542d3639302056312e32008a020d0a"));
 
-        verifyNotNull(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040450004323132474c31313433303035303033000000000040082ca89b55a6a99b555c57000000000000c40200000b0000001400036401111f000302f5533bd653f10d0a"));
 
         verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,
@@ -81,7 +81,7 @@ public class CastelProtocolDecoderTest extends ProtocolTest {
         verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040420004323132474c31313433303035303033000000000010022ca89b55cca99b555c57000000000000cf0200000b0000000000036401111f0000020013be0d0a"));
 
-        verifyNotNull(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040870004323132474c31313433303035303033000000000040052ca89b55e3a89b555c57000000000000c4020000040000001400036401111f0003000012042105210b210c210d210f211021112113211c2121212321242133213421422146214f212b50663603003ce9030dff060000600dffffc25865ffff9e02b43624000000003cbc0d0a"));
 
         verifyNothing(decoder, binary(ByteOrder.LITTLE_ENDIAN,