Add support for G sensor

1 files changed, 30 insertions, 1 deletions

diff --git a/src/main/java/org/traccar/protocol/CastelProtocolDecoder.java b/src/main/java/org/traccar/protocol/CastelProtocolDecoder.java
index 1f67b6a77..03e4b25fd 100644
--- a/src/main/java/org/traccar/protocol/CastelProtocolDecoder.java
+++ b/src/main/java/org/traccar/protocol/CastelProtocolDecoder.java
@@ -292,6 +292,7 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
             int version, ByteBuf id, short type, DeviceSession deviceSession) {
 
         Position position;
+        int count;
 
         switch (type) {
 
@@ -330,7 +331,7 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
                 long status = buf.readUnsignedIntLE();
                 buf.skipBytes(8);
 
-                int count = buf.readUnsignedByte();
+                count = buf.readUnsignedByte();
 
                 List<Position> positions = new LinkedList<>();
 
@@ -380,6 +381,34 @@ public class CastelProtocolDecoder extends BaseProtocolDecoder {
 
                 return position;
 
+            case MSG_SC_G_SENSOR:
+                position = createPosition(deviceSession);
+
+                decodeStat(position, buf);
+
+                buf.readUnsignedShortLE(); // sample rate
+
+                count = buf.readUnsignedByte();
+
+                StringBuilder data = new StringBuilder("[");
+                for (int i = 0; i < count; i++) {
+                    if (i > 0) {
+                        data.append(",");
+                    }
+                    data.append("[");
+                    data.append(buf.readShortLE() * 0.015625);
+                    data.append(",");
+                    data.append(buf.readShortLE() * 0.015625);
+                    data.append(",");
+                    data.append(buf.readShortLE() * 0.015625);
+                    data.append("]");
+                }
+                data.append("]");
+
+                position.set(Position.KEY_G_SENSOR, data.toString());
+
+                return position;
+
             case MSG_SC_DTCS_PASSENGER:
                 position = createPosition(deviceSession);