Better Teltonika support (fix #3988)

1 files changed, 117 insertions, 137 deletions

diff --git a/src/main/java/org/traccar/protocol/TeltonikaProtocolDecoder.java b/src/main/java/org/traccar/protocol/TeltonikaProtocolDecoder.java
index a7c46402f..2b1196d55 100644
--- a/src/main/java/org/traccar/protocol/TeltonikaProtocolDecoder.java
+++ b/src/main/java/org/traccar/protocol/TeltonikaProtocolDecoder.java
@@ -20,6 +20,7 @@ import io.netty.buffer.ByteBufUtil;
 import io.netty.buffer.Unpooled;
 import io.netty.channel.Channel;
 import org.traccar.BaseProtocolDecoder;
+import org.traccar.model.Device;
 import org.traccar.session.DeviceSession;
 import org.traccar.NetworkMessage;
 import org.traccar.Protocol;
@@ -38,11 +39,15 @@ import java.util.HashMap;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.Map;
+import java.util.Set;
+import java.util.function.BiConsumer;
 
 public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
 
     private static final int IMAGE_PACKET_MAX = 2048;
 
+    private static final Map<Integer, Map<Set<String>, BiConsumer<Position, ByteBuf>>> PARAMETERS = new HashMap<>();
+
     private final boolean connectionless;
     private boolean extended;
     private final Map<Long, ByteBuf> photos = new HashMap<>();
@@ -184,185 +189,160 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         }
     }
 
-    private long readValue(ByteBuf buf, int length, boolean signed) {
+    private long readValue(ByteBuf buf, int length) {
         switch (length) {
             case 1:
-                return signed ? buf.readByte() : buf.readUnsignedByte();
+                return buf.readUnsignedByte();
             case 2:
-                return signed ? buf.readShort() : buf.readUnsignedShort();
+                return buf.readUnsignedShort();
             case 4:
-                return signed ? buf.readInt() : buf.readUnsignedInt();
+                return buf.readUnsignedInt();
             default:
                 return buf.readLong();
         }
     }
 
-    private void decodeUniversalParameter(Position position, int id, ByteBuf buf, int length) {
-        switch (id) {
-            case 1:
-            case 2:
-            case 3:
-            case 4:
-                position.set(Position.PREFIX_IN + id, readValue(buf, length, false));
-                break;
-            case 9:
-                position.set(Position.PREFIX_ADC + 1, readValue(buf, length, false));
-                break;
-            case 10:
-                position.set(Position.PREFIX_ADC + 2, readValue(buf, length, false));
-                break;
-            case 16:
-                position.set(Position.KEY_ODOMETER, readValue(buf, length, false));
-                break;
-            case 17:
-                position.set("axisX", readValue(buf, length, true));
-                break;
-            case 18:
-                position.set("axisY", readValue(buf, length, true));
-                break;
-            case 19:
-                position.set("axisZ", readValue(buf, length, true));
-                break;
-            case 21:
-                position.set(Position.KEY_RSSI, readValue(buf, length, false));
-                break;
-            case 25:
-            case 26:
-            case 27:
-            case 28:
-                position.set(Position.PREFIX_TEMP + (id - 24 + 4), readValue(buf, length, true) * 0.1);
-                break;
-            case 66:
-                position.set(Position.KEY_POWER, readValue(buf, length, false) * 0.001);
-                break;
-            case 67:
-                position.set(Position.KEY_BATTERY, readValue(buf, length, false) * 0.001);
-                break;
-            case 72:
-            case 73:
-            case 74:
-                position.set(Position.PREFIX_TEMP + (id - 71), readValue(buf, length, true) * 0.1);
-                break;
-            case 78:
-                long driverUniqueId = readValue(buf, length, false);
-                if (driverUniqueId != 0) {
-                    position.set(Position.KEY_DRIVER_UNIQUE_ID, String.format("%016X", driverUniqueId));
-                }
-                break;
-            case 80:
-                position.set("workMode", readValue(buf, length, false));
-                break;
-            case 90:
-                position.set(Position.KEY_DOOR, readValue(buf, length, false));
-                break;
-            case 115:
-                position.set(Position.KEY_COOLANT_TEMP, readValue(buf, length, true) * 0.1);
-                break;
-            case 179:
-                position.set(Position.PREFIX_OUT + 1, readValue(buf, length, false) == 1);
-                break;
-            case 180:
-                position.set(Position.PREFIX_OUT + 2, readValue(buf, length, false) == 1);
-                break;
-            case 181:
-                position.set(Position.KEY_PDOP, readValue(buf, length, false) * 0.1);
-                break;
-            case 182:
-                position.set(Position.KEY_HDOP, readValue(buf, length, false) * 0.1);
-                break;
-            case 199:
-                position.set(Position.KEY_ODOMETER_TRIP, readValue(buf, length, false));
-                break;
-            case 236:
-                if (readValue(buf, length, false) == 1) {
-                    position.set(Position.KEY_ALARM, Position.ALARM_GENERAL);
-                }
-                break;
-            case 239:
-                position.set(Position.KEY_IGNITION, readValue(buf, length, false) == 1);
-                break;
-            case 240:
-                position.set(Position.KEY_MOTION, readValue(buf, length, false) == 1);
-                break;
-            case 241:
-                position.set(Position.KEY_OPERATOR, readValue(buf, length, false));
-                break;
-            case 253:
-                switch ((int) readValue(buf, length, false)) {
-                    case 1:
-                        position.set(Position.KEY_ALARM, Position.ALARM_ACCELERATION);
-                        break;
-                    case 2:
-                        position.set(Position.KEY_ALARM, Position.ALARM_BRAKING);
-                        break;
-                    case 3:
-                        position.set(Position.KEY_ALARM, Position.ALARM_CORNERING);
-                        break;
-                    default:
-                        break;
-                }
-                break;
-            default:
-                position.set(Position.PREFIX_IO + id, readValue(buf, length, false));
-                break;
-        }
+    private static void register(int id, Set<String> models, BiConsumer<Position, ByteBuf> handler) {
+        PARAMETERS.computeIfAbsent(id, key -> new HashMap<>()).put(models, handler);
+    }
+
+    static {
+        var fmbXXX = Set.of(
+                "FMB001", "FMB010", "FMB002", "FMB020", "FMB003", "FMB110", "FMB120", "FMB122", "FMB125", "FMB130",
+                "FMB140", "FMU125", "FMB900", "FMB920", "FMB962", "FMB964", "FM3001", "FMB202", "FMB204", "FMB206",
+                "FMT100", "MTB100", "FMP100", "MSP500");
+
+        register(1, null, (p, b) -> p.set(Position.PREFIX_IN + 1, b.readUnsignedByte() > 0));
+        register(2, null, (p, b) -> p.set(Position.PREFIX_IN + 2, b.readUnsignedByte() > 0));
+        register(3, null, (p, b) -> p.set(Position.PREFIX_IN + 3, b.readUnsignedByte() > 0));
+        register(4, null, (p, b) -> p.set(Position.PREFIX_IN + 4, b.readUnsignedByte() > 0));
+        register(9, fmbXXX, (p, b) -> p.set(Position.PREFIX_ADC + 1, b.readUnsignedShort() * 0.001));
+        register(10, fmbXXX, (p, b) -> p.set(Position.PREFIX_ADC + 2, b.readUnsignedShort() * 0.001));
+        register(11, fmbXXX, (p, b) -> p.set(Position.KEY_ICCID, String.valueOf(b.readLong())));
+        register(12, fmbXXX, (p, b) -> p.set(Position.KEY_FUEL_USED, b.readUnsignedInt() * 0.001));
+        register(13, fmbXXX, (p, b) -> p.set(Position.KEY_FUEL_CONSUMPTION, b.readUnsignedShort() * 0.01));
+        register(16, null, (p, b) -> p.set(Position.KEY_ODOMETER, b.readUnsignedInt()));
+        register(17, null, (p, b) -> p.set("axisX", b.readShort()));
+        register(18, null, (p, b) -> p.set("axisY", b.readShort()));
+        register(19, null, (p, b) -> p.set("axisZ", b.readShort()));
+        register(21, null, (p, b) -> p.set(Position.KEY_RSSI, b.readUnsignedByte()));
+        register(24, fmbXXX, (p, b) -> p.setSpeed(UnitsConverter.knotsFromKph(b.readUnsignedShort())));
+        register(25, null, (p, b) -> p.set("bleTemp1", b.readShort() * 0.01));
+        register(26, null, (p, b) -> p.set("bleTemp2", b.readShort() * 0.01));
+        register(27, null, (p, b) -> p.set("bleTemp3", b.readShort() * 0.01));
+        register(28, null, (p, b) -> p.set("bleTemp4", b.readShort() * 0.01));
+        register(66, null, (p, b) -> p.set(Position.KEY_POWER, b.readUnsignedShort() * 0.001));
+        register(67, null, (p, b) -> p.set(Position.KEY_BATTERY, b.readUnsignedShort() * 0.001));
+        register(68, fmbXXX, (p, b) -> p.set("batteryCurrent", b.readUnsignedShort() * 0.001));
+        register(72, fmbXXX, (p, b) -> p.set(Position.PREFIX_TEMP + 1, b.readShort() * 0.1));
+        register(73, fmbXXX, (p, b) -> p.set(Position.PREFIX_TEMP + 2, b.readShort() * 0.1));
+        register(74, fmbXXX, (p, b) -> p.set(Position.PREFIX_TEMP + 3, b.readShort() * 0.1));
+        register(75, fmbXXX, (p, b) -> p.set(Position.PREFIX_TEMP + 4, b.readShort() * 0.1));
+        register(78, null, (p, b) -> {
+            long driverUniqueId = b.readLong();
+            if (driverUniqueId > 0) {
+                p.set(Position.KEY_DRIVER_UNIQUE_ID, String.format("%016X", driverUniqueId));
+            }
+        });
+        register(80, null, (p, b) -> p.set("dataMode", b.readUnsignedByte()));
+        register(90, null, (p, b) -> p.set(Position.KEY_DOOR, b.readUnsignedShort()));
+        register(115, fmbXXX, (p, b) -> p.set(Position.KEY_COOLANT_TEMP, b.readShort() * 0.1));
+        register(179, null, (p, b) -> p.set(Position.PREFIX_OUT + 1, b.readUnsignedByte() > 0));
+        register(180, null, (p, b) -> p.set(Position.PREFIX_OUT + 2, b.readUnsignedByte() > 0));
+        register(181, null, (p, b) -> p.set(Position.KEY_PDOP, b.readUnsignedShort() * 0.1));
+        register(182, null, (p, b) -> p.set(Position.KEY_HDOP, b.readUnsignedShort() * 0.1));
+        register(199, null, (p, b) -> p.set(Position.KEY_ODOMETER_TRIP, b.readUnsignedInt()));
+        register(200, fmbXXX, (p, b) -> p.set("sleepMode", b.readUnsignedByte()));
+        register(205, null, (p, b) -> p.set("cid", b.readUnsignedShort()));
+        register(206, null, (p, b) -> p.set("lac", b.readUnsignedShort()));
+        register(236, null, (p, b) -> {
+            p.set(Position.KEY_ALARM, b.readUnsignedByte() > 0 ? Position.ALARM_GENERAL : null);
+        });
+        register(239, null, (p, b) -> p.set(Position.KEY_IGNITION, b.readUnsignedByte() > 0));
+        register(240, null, (p, b) -> p.set(Position.KEY_MOTION, b.readUnsignedByte() > 0));
+        register(241, null, (p, b) -> p.set(Position.KEY_OPERATOR, b.readUnsignedInt()));
+        register(253, null, (p, b) -> {
+            switch (b.readUnsignedByte()) {
+                case 1:
+                    p.set(Position.KEY_ALARM, Position.ALARM_ACCELERATION);
+                    break;
+                case 2:
+                    p.set(Position.KEY_ALARM, Position.ALARM_BRAKING);
+                    break;
+                case 3:
+                    p.set(Position.KEY_ALARM, Position.ALARM_CORNERING);
+                    break;
+                default:
+                    break;
+            }
+        });
     }
 
     private void decodeGh3000Parameter(Position position, int id, ByteBuf buf, int length) {
         switch (id) {
             case 1:
-                position.set(Position.KEY_BATTERY_LEVEL, readValue(buf, length, false));
+                position.set(Position.KEY_BATTERY_LEVEL, readValue(buf, length));
                 break;
             case 2:
-                position.set("usbConnected", readValue(buf, length, false) == 1);
+                position.set("usbConnected", readValue(buf, length) == 1);
                 break;
             case 5:
-                position.set("uptime", readValue(buf, length, false));
+                position.set("uptime", readValue(buf, length));
                 break;
             case 20:
-                position.set(Position.KEY_HDOP, readValue(buf, length, false) * 0.1);
+                position.set(Position.KEY_HDOP, readValue(buf, length) * 0.1);
                 break;
             case 21:
-                position.set(Position.KEY_VDOP, readValue(buf, length, false) * 0.1);
+                position.set(Position.KEY_VDOP, readValue(buf, length) * 0.1);
                 break;
             case 22:
-                position.set(Position.KEY_PDOP, readValue(buf, length, false) * 0.1);
+                position.set(Position.KEY_PDOP, readValue(buf, length) * 0.1);
                 break;
             case 67:
-                position.set(Position.KEY_BATTERY, readValue(buf, length, false) * 0.001);
+                position.set(Position.KEY_BATTERY, readValue(buf, length) * 0.001);
                 break;
             case 221:
-                position.set("button", readValue(buf, length, false));
+                position.set("button", readValue(buf, length));
                 break;
             case 222:
-                if (readValue(buf, length, false) == 1) {
+                if (readValue(buf, length) == 1) {
                     position.set(Position.KEY_ALARM, Position.ALARM_SOS);
                 }
                 break;
             case 240:
-                position.set(Position.KEY_MOTION, readValue(buf, length, false) == 1);
+                position.set(Position.KEY_MOTION, readValue(buf, length) == 1);
                 break;
             case 244:
-                position.set(Position.KEY_ROAMING, readValue(buf, length, false) == 1);
+                position.set(Position.KEY_ROAMING, readValue(buf, length) == 1);
                 break;
             default:
-                position.set(Position.PREFIX_IO + id, readValue(buf, length, false));
+                position.set(Position.PREFIX_IO + id, readValue(buf, length));
                 break;
         }
     }
 
-    private void decodeParameter(Position position, int id, ByteBuf buf, int length, int codec) {
+    private void decodeParameter(Position position, int id, ByteBuf buf, int length, int codec, String model) {
         if (codec == CODEC_GH3000) {
             decodeGh3000Parameter(position, id, buf, length);
         } else {
-            decodeUniversalParameter(position, id, buf, length);
+            boolean decoded = false;
+            for (var entry : PARAMETERS.getOrDefault(id, new HashMap<>()).entrySet()) {
+                if (entry.getKey() == null || model != null && entry.getKey().contains(model)) {
+                    entry.getValue().accept(position, buf);
+                    decoded = true;
+                    break;
+                }
+            }
+            if (!decoded) {
+                position.set(Position.PREFIX_IO + id, readValue(buf, length));
+            }
         }
     }
 
     private void decodeNetwork(Position position) {
-        long cid = position.getLong(Position.PREFIX_IO + 205);
-        int lac = position.getInteger(Position.PREFIX_IO + 206);
-        if (cid != 0 && lac != 0) {
+        Integer cid = (Integer) position.getAttributes().remove("cid");
+        Integer lac = (Integer) position.getAttributes().remove("lac");
+        if (cid != null && lac != null) {
             CellTower cellTower = CellTower.fromLacCid(getConfig(), lac, cid);
             long operator = position.getInteger(Position.KEY_OPERATOR);
             if (operator >= 1000) {
@@ -387,7 +367,7 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         }
     }
 
-    private void decodeLocation(Position position, ByteBuf buf, int codec) {
+    private void decodeLocation(Position position, ByteBuf buf, int codec, String model) {
 
         int globalMask = 0x0f;
 
@@ -484,7 +464,7 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         if (BitUtil.check(globalMask, 1)) {
             int cnt = readExtByte(buf, codec, CODEC_8_EXT);
             for (int j = 0; j < cnt; j++) {
-                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 1, codec);
+                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 1, codec, model);
             }
         }
 
@@ -492,7 +472,7 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         if (BitUtil.check(globalMask, 2)) {
             int cnt = readExtByte(buf, codec, CODEC_8_EXT);
             for (int j = 0; j < cnt; j++) {
-                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 2, codec);
+                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 2, codec, model);
             }
         }
 
@@ -500,7 +480,7 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         if (BitUtil.check(globalMask, 3)) {
             int cnt = readExtByte(buf, codec, CODEC_8_EXT);
             for (int j = 0; j < cnt; j++) {
-                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 4, codec);
+                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 4, codec, model);
             }
         }
 
@@ -508,7 +488,7 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         if (codec == CODEC_8 || codec == CODEC_8_EXT || codec == CODEC_16) {
             int cnt = readExtByte(buf, codec, CODEC_8_EXT);
             for (int j = 0; j < cnt; j++) {
-                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 8, codec);
+                decodeParameter(position, readExtByte(buf, codec, CODEC_8_EXT, CODEC_16), buf, 8, codec, model);
             }
         }
 
@@ -578,10 +558,10 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
         int count = buf.readUnsignedByte();
 
         DeviceSession deviceSession = getDeviceSession(channel, remoteAddress, imei);
-
         if (deviceSession == null) {
             return null;
         }
+        String model = getCacheManager().getObject(Device.class, deviceSession.getDeviceId()).getModel();
 
         for (int i = 0; i < count; i++) {
             Position position = new Position(getProtocolName());
@@ -603,7 +583,7 @@ public class TeltonikaProtocolDecoder extends BaseProtocolDecoder {
             } else if (codec == CODEC_12) {
                 decodeSerial(channel, remoteAddress, deviceSession, position, buf);
             } else {
-                decodeLocation(position, buf, codec);
+                decodeLocation(position, buf, codec, model);
             }
 
             if (!position.getOutdated() || !position.getAttributes().isEmpty()) {