Decode telemetry messages

2 files changed, 194 insertions, 29 deletions

diff --git a/src/main/java/org/traccar/protocol/NavtelecomProtocolDecoder.java b/src/main/java/org/traccar/protocol/NavtelecomProtocolDecoder.java
index 0e9685356..c469025c2 100644
--- a/src/main/java/org/traccar/protocol/NavtelecomProtocolDecoder.java
+++ b/src/main/java/org/traccar/protocol/NavtelecomProtocolDecoder.java
@@ -19,12 +19,22 @@ import io.netty.buffer.ByteBuf;
 import io.netty.buffer.Unpooled;
 import io.netty.channel.Channel;
 import org.traccar.BaseProtocolDecoder;
+import org.traccar.DeviceSession;
 import org.traccar.NetworkMessage;
 import org.traccar.Protocol;
+import org.traccar.helper.BitUtil;
 import org.traccar.helper.Checksum;
+import org.traccar.helper.UnitsConverter;
+import org.traccar.model.Position;
 
 import java.net.SocketAddress;
 import java.nio.charset.StandardCharsets;
+import java.util.BitSet;
+import java.util.Date;
+import java.util.HashMap;
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Map;
 
 public class NavtelecomProtocolDecoder extends BaseProtocolDecoder {
 
@@ -32,6 +42,67 @@ public class NavtelecomProtocolDecoder extends BaseProtocolDecoder {
         super(protocol);
     }
 
+    private static final Map<Integer, Integer> ITEM_LENGTH_MAP = new HashMap<>();
+
+    static {
+        int[] l1 = {
+                4, 5, 6, 7, 8, 29, 30, 31, 32, 45, 46, 47, 48, 49, 50, 51, 52, 56, 63, 64, 65, 69, 72, 78, 79, 80, 81,
+                82, 83, 98, 99, 101, 104, 118, 122, 123, 124, 125, 126, 139, 140, 144, 145, 167, 168, 169, 170, 199,
+                202, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222
+        };
+        int[] l2 = {
+                2, 14, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 35, 36, 38, 39, 40, 41, 42, 43, 44, 53, 55, 58,
+                59, 60, 61, 62, 66, 68, 71, 75, 100, 106, 108, 110, 111, 112, 113, 114, 115, 116, 117, 119, 120, 121,
+                133, 134, 135, 136, 137, 138, 141, 143, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
+                160, 161, 162, 163, 164, 165, 166, 171, 175, 177, 178, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189,
+                190, 191, 192, 200, 201, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237
+        };
+        int[] l3 = {
+                84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 142, 146, 198
+        };
+        int[] l4 = {
+                1, 3, 9, 10, 11, 12, 13, 15, 16, 33, 34, 37, 54, 57, 67, 74, 76, 102, 103, 105, 127, 128, 129, 130, 131,
+                132, 172, 173, 174, 176, 179, 193, 194, 195, 196, 203, 205, 206, 238, 239, 240, 241, 242, 243, 244, 245,
+                246, 247, 248, 249, 250, 251, 252
+        };
+        for (int i : l1) {
+            ITEM_LENGTH_MAP.put(i, 1);
+        }
+        for (int i : l2) {
+            ITEM_LENGTH_MAP.put(i, 2);
+        }
+        for (int i : l3) {
+            ITEM_LENGTH_MAP.put(i, 3);
+        }
+        for (int i : l4) {
+            ITEM_LENGTH_MAP.put(i, 4);
+        }
+        ITEM_LENGTH_MAP.put(70, 8);
+        ITEM_LENGTH_MAP.put(73, 16);
+        ITEM_LENGTH_MAP.put(77, 37);
+        ITEM_LENGTH_MAP.put(94, 6);
+        ITEM_LENGTH_MAP.put(95, 12);
+        ITEM_LENGTH_MAP.put(96, 24);
+        ITEM_LENGTH_MAP.put(97, 48);
+        ITEM_LENGTH_MAP.put(107, 6);
+        ITEM_LENGTH_MAP.put(109, 6);
+        ITEM_LENGTH_MAP.put(197, 6);
+        ITEM_LENGTH_MAP.put(204, 5);
+        ITEM_LENGTH_MAP.put(253, 8);
+        ITEM_LENGTH_MAP.put(254, 8);
+        ITEM_LENGTH_MAP.put(255, 8);
+    }
+
+    private BitSet bits;
+
+    private static int getItemLength(int id) {
+        Integer length = ITEM_LENGTH_MAP.get(id);
+        if (length == null) {
+            throw new IllegalArgumentException(String.format("Unknown item: %d", id));
+        }
+        return length;
+    }
+
     private void sendResponse(
             Channel channel, SocketAddress remoteAddress, int receiver, int sender, ByteBuf content) {
         if (channel != null) {
@@ -55,35 +126,123 @@ public class NavtelecomProtocolDecoder extends BaseProtocolDecoder {
 
         ByteBuf buf = (ByteBuf) msg;
 
-        buf.skipBytes(4); // preamble
-        int receiver = buf.readIntLE();
-        int sender = buf.readIntLE();
-        int length = buf.readUnsignedShortLE();
-        buf.readUnsignedByte(); // data checksum
-        buf.readUnsignedByte(); // header checksum
-
-        String type = buf.toString(buf.readerIndex(), 6, StandardCharsets.US_ASCII);
-
-        if (type.startsWith("*>S")) {
-
-            String sentence = buf.readCharSequence(length, StandardCharsets.US_ASCII).toString();
-            getDeviceSession(channel, remoteAddress, sentence.substring(4));
-
-            ByteBuf payload = Unpooled.copiedBuffer("*<S", StandardCharsets.US_ASCII);
-
-            sendResponse(channel, remoteAddress, receiver, sender, payload);
-
-        } else if (type.startsWith("*>FLEX")) {
-
-            buf.skipBytes(6);
-
-            ByteBuf payload = Unpooled.buffer();
-            payload.writeCharSequence("*<FLEX", StandardCharsets.US_ASCII);
-            payload.writeByte(buf.readUnsignedByte()); // protocol
-            payload.writeByte(buf.readUnsignedByte()); // protocol version
-            payload.writeByte(buf.readUnsignedByte()); // struct version
-
-            sendResponse(channel, remoteAddress, receiver, sender, payload);
+        if (buf.getByte(buf.readerIndex()) == '@') {
+
+            buf.skipBytes(4); // preamble
+            int receiver = buf.readIntLE();
+            int sender = buf.readIntLE();
+            int length = buf.readUnsignedShortLE();
+            buf.readUnsignedByte(); // data checksum
+            buf.readUnsignedByte(); // header checksum
+
+            String type = buf.toString(buf.readerIndex(), 6, StandardCharsets.US_ASCII);
+
+            if (type.startsWith("*>S")) {
+
+                String sentence = buf.readCharSequence(length, StandardCharsets.US_ASCII).toString();
+                getDeviceSession(channel, remoteAddress, sentence.substring(4));
+
+                ByteBuf payload = Unpooled.copiedBuffer("*<S", StandardCharsets.US_ASCII);
+
+                sendResponse(channel, remoteAddress, receiver, sender, payload);
+
+            } else if (type.startsWith("*>FLEX")) {
+
+                buf.skipBytes(6);
+
+                ByteBuf payload = Unpooled.buffer();
+                payload.writeCharSequence("*<FLEX", StandardCharsets.US_ASCII);
+                payload.writeByte(buf.readUnsignedByte()); // protocol
+                payload.writeByte(buf.readUnsignedByte()); // protocol version
+                payload.writeByte(buf.readUnsignedByte()); // struct version
+
+                int bitCount = buf.readUnsignedByte();
+                bits = new BitSet((bitCount + 7) / 8);
+
+                int currentByte = 0;
+                for (int i = 0; i < bitCount; i++) {
+                    if (i % 8 == 0) {
+                        currentByte = buf.readUnsignedByte();
+                    }
+                    bits.set(i, BitUtil.check(currentByte, 7 - i % 8));
+                }
+
+                sendResponse(channel, remoteAddress, receiver, sender, payload);
+
+            }
+
+        } else {
+
+            DeviceSession deviceSession = getDeviceSession(channel, remoteAddress);
+            if (deviceSession == null) {
+                return null;
+            }
+
+            String type = buf.readCharSequence(2, StandardCharsets.US_ASCII).toString();
+
+            if (type.equals("~A")) {
+
+                int count = buf.readUnsignedByte();
+                List<Position> positions = new LinkedList<>();
+
+                for (int i = 0; i < count; i++) {
+                    Position position = new Position(getProtocolName());
+                    position.setDeviceId(deviceSession.getDeviceId());
+
+                    for (int j = 0; j < bits.length(); j++) {
+                        if (bits.get(j)) {
+                            switch (j + 1) {
+                                case 1:
+                                    position.set(Position.KEY_INDEX, buf.readUnsignedIntLE());
+                                    break;
+                                case 2:
+                                    position.set(Position.KEY_EVENT, buf.readUnsignedShortLE());
+                                    break;
+                                case 3:
+                                    position.setDeviceTime(new Date(buf.readUnsignedIntLE() * 1000));
+                                    break;
+                                case 9:
+                                    position.setValid(true);
+                                    position.setFixTime(new Date(buf.readUnsignedIntLE() * 1000));
+                                    break;
+                                case 10:
+                                    position.setLatitude(buf.readIntLE() * 0.0001 / 60);
+                                    break;
+                                case 11:
+                                    position.setLongitude(buf.readIntLE() * 0.0001 / 60);
+                                    break;
+                                case 12:
+                                    position.setAltitude(buf.readIntLE() * 0.1);
+                                    break;
+                                case 13:
+                                    position.setSpeed(UnitsConverter.knotsFromKph(buf.readFloatLE()));
+                                    break;
+                                default:
+                                    buf.skipBytes(getItemLength(j + 1));
+                                    break;
+                            }
+                        }
+                    }
+
+                    if (position.getFixTime() == null) {
+                        getLastLocation(position, position.getDeviceTime());
+                    }
+
+                    positions.add(position);
+                }
+
+                int checksum = buf.readUnsignedByte();
+                if (channel != null) {
+                    ByteBuf response = Unpooled.buffer();
+                    response.writeCharSequence(type, StandardCharsets.US_ASCII);
+                    response.writeByte(count);
+                    response.writeByte(checksum);
+                    channel.writeAndFlush(new NetworkMessage(response, remoteAddress));
+                }
+
+                return positions;
+
+            }
 
         }
 
diff --git a/src/test/java/org/traccar/protocol/NavtelecomProtocolDecoderTest.java b/src/test/java/org/traccar/protocol/NavtelecomProtocolDecoderTest.java
index be51cb852..fd22049fc 100644
--- a/src/test/java/org/traccar/protocol/NavtelecomProtocolDecoderTest.java
+++ b/src/test/java/org/traccar/protocol/NavtelecomProtocolDecoderTest.java
@@ -14,6 +14,12 @@ public class NavtelecomProtocolDecoderTest extends ProtocolTest {
                 "404e5443010000000000000013004e452a3e533a383636373935303331343130363839"));
 
         verifyNull(decoder, binary(
+                "404e544301000000000000002a005e6c2a3e464c4558b01e1efffffe300a08080ffffe08000000580028002bc0000000000000b4000000000000"));
+
+        verifyPositions(decoder, binary(
+                "7e4104022106000517c4ae2f6180a9000e2fc4ae2f61471dff0171b35801d2050000a9870e412801d9d096466a37061000009474270080ff7f00000000ffff8000000000ffffffffffffffffffffffffffff7f00000000ffffff0308000000000000090cf70900000826fa000200b3ad2b00000826fa000200aad75200000826fa000200aa9cae2f6158020000000000000000000a14000000000000000000000000000000000000000026000000032106000b17dbae2f6180a9000e33daae2f61a11dff01edb15801d00500009c50e83f2f01ecd09646793706100000ab74270080ff7f00000000ffff8000000000ffffffffffffffffffffffffffff7f00000000ffffff0408000000000000090bf70900000826fa000200af8bc70000256cfa000200ab3e7c0000256cfa000200aad7ae2f61fd080000000000000000000a140100000000000000000000000000000000000000260000000421060054a0e7ae2f6180a9000e33e6ae2f61ba1dff01beb15801d305000038b977402201f0d09646163706100000b674270080ff7f00000000ffff8000000000ffffffffffffffffffffffffffff7f00000000ffffff0309000000000000080bf70900000826fa000200af8bc70000256cfa000200ab3e7c0000256cfa000200aad7ae2f6173040000000000000000000a14080000000000000000000000000000000000000026000000052106000517efae2f6180a9000f33efae2f61c21dff0166b15801df05000017f145404d00f5d09646693706100000bf74270080ff7f00000000ffff8000000000ffffffffffffffffffffffffffff7f00000000ffffff0408000000000000090cf70900000826fa000200af8bc70000256cfa000200ab3e7c0000256cfa000200aad7ae2f615b030000000000000000000a14020000000000000000000000000000000000000026000000a9"));
+
+        verifyNull(decoder, binary(
                 "404e544301000000000000001300f7fc2a3e464c4558b00a0a45fffe00000000000000"));
 
         verifyNull(decoder, binary(