2 files changed, 60 insertions, 5 deletions

diff --git a/src/org/traccar/helper/ObdDecoder.java b/src/org/traccar/helper/ObdDecoder.java
index 4686aa682..6ffe39662 100644
--- a/src/org/traccar/helper/ObdDecoder.java
+++ b/src/org/traccar/helper/ObdDecoder.java
@@ -25,9 +25,9 @@ public final class ObdDecoder {
     private ObdDecoder() {
     }
 
-    public static final int MODE_CURRENT = 0x01;
-    public static final int MODE_FREEZE_FRAME = 0x02;
-    public static final int MODE_CODES = 0x03;
+    private static final int MODE_CURRENT = 0x01;
+    private static final int MODE_FREEZE_FRAME = 0x02;
+    private static final int MODE_CODES = 0x03;
 
     private static final int PID_ENGINE_LOAD = 0x04;
     private static final int PID_COOLANT_TEMPERATURE = 0x05;
@@ -54,7 +54,7 @@ public final class ObdDecoder {
         return new AbstractMap.SimpleEntry<>(key, value);
     }
 
-    private static Map.Entry<String, Object> decodeCodes(String value) {
+    public static Map.Entry<String, Object> decodeCodes(String value) {
         StringBuilder codes = new StringBuilder();
         for (int i = 0; i < value.length() / 4; i++) {
             int numValue = Integer.parseInt(value.substring(i * 4, (i + 1) * 4), 16);
@@ -75,7 +75,11 @@ public final class ObdDecoder {
             }
             codes.append(String.format("%04X", numValue & 0x3FFF));
         }
-        return createEntry("dtcs", codes.toString().replaceFirst(",", ""));
+        if (codes.length() > 0) {
+            return createEntry("dtcs", codes.toString().replaceFirst(",", ""));
+        } else {
+            return null;
+        }
     }
 
     private static Map.Entry<String, Object> decodeData(int pid, String value) {
diff --git a/src/org/traccar/protocol/CastelProtocolDecoder.java b/src/org/traccar/protocol/CastelProtocolDecoder.java
index 38fb11164..b8faed077 100644
--- a/src/org/traccar/protocol/CastelProtocolDecoder.java
+++ b/src/org/traccar/protocol/CastelProtocolDecoder.java
@@ -22,6 +22,7 @@ import org.traccar.BaseProtocolDecoder;
 import org.traccar.DeviceSession;
 import org.traccar.helper.Checksum;
 import org.traccar.helper.DateBuilder;
+import org.traccar.helper.ObdDecoder;
 import org.traccar.helper.UnitsConverter;
 import org.traccar.model.Position;
 
@@ -43,7 +44,13 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
     private static final short MSG_SC_HEARTBEAT = 0x1003;
     private static final short MSG_SC_HEARTBEAT_RESPONSE = (short) 0x9003;
     private static final short MSG_SC_GPS = 0x4001;
+    private static final short MSG_SC_PID_DATA = 0x4002;
+    private static final short MSG_SC_SUPPORTED_PID = 0x4004;
+    private static final short MSG_SC_OBD_DATA = 0x4005;
+    private static final short MSG_SC_DTCS_PASSENGER = 0x4006;
+    private static final short MSG_SC_DTCS_COMMERCIAL = 0x400B;
     private static final short MSG_SC_ALARM = 0x4007;
+    private static final short MSG_SC_CELL = 0x4008;
     private static final short MSG_SC_GPS_SLEEP = 0x4009;
     private static final short MSG_SC_AGPS_REQUEST = 0x5101;
     private static final short MSG_SC_CURRENT_LOCATION = (short) 0xB001;
@@ -193,6 +200,50 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
             return readPosition(deviceSession, buf);
 
+        } else if (type == MSG_SC_DTCS_PASSENGER) {
+
+            Position position = new Position();
+            position.setProtocol(getProtocolName());
+            position.setDeviceId(deviceSession.getDeviceId());
+
+            getLastLocation(position, null);
+
+            buf.skipBytes(6 * 4 + 2 + 8);
+
+            buf.readUnsignedByte(); // flag
+            position.add(ObdDecoder.decodeCodes(ChannelBuffers.hexDump(buf.readBytes(buf.readUnsignedByte()))));
+
+            return position;
+
+        } else if (type == MSG_SC_OBD_DATA) {
+
+            Position position = new Position();
+            position.setProtocol(getProtocolName());
+            position.setDeviceId(deviceSession.getDeviceId());
+
+            getLastLocation(position, null);
+
+            buf.skipBytes(6 * 4 + 2 + 8);
+
+            // decode data
+
+            return position;
+
+        } else if (type == MSG_SC_CELL) {
+
+            Position position = new Position();
+            position.setProtocol(getProtocolName());
+            position.setDeviceId(deviceSession.getDeviceId());
+
+            getLastLocation(position, null);
+
+            buf.skipBytes(6 * 4 + 2 + 8);
+
+            position.set(Position.KEY_LAC, buf.readUnsignedShort());
+            position.set(Position.KEY_CID, buf.readUnsignedShort());
+
+            return position;
+
         }
 
         return null;