Extend KHD protocol support

1 files changed, 29 insertions, 11 deletions

diff --git a/src/org/traccar/protocol/KhdProtocolDecoder.java b/src/org/traccar/protocol/KhdProtocolDecoder.java
index 0c3d8ee51..1c88a8b9d 100644
--- a/src/org/traccar/protocol/KhdProtocolDecoder.java
+++ b/src/org/traccar/protocol/KhdProtocolDecoder.java
@@ -1,5 +1,5 @@
 /*
- * Copyright 2014 Anton Tananaev (anton@traccar.org)
+ * Copyright 2014 - 2017 Anton Tananaev (anton@traccar.org)
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -93,24 +93,42 @@ public class KhdProtocolDecoder extends BaseProtocolDecoder {
             position.setLongitude(BcdUtil.readCoordinate(buf));
             position.setSpeed(UnitsConverter.knotsFromKph(BcdUtil.readInteger(buf, 4)));
             position.setCourse(BcdUtil.readInteger(buf, 4));
+            position.setValid((buf.readUnsignedByte() & 0x80) != 0);
 
-            int flags = buf.readUnsignedByte();
-            position.setValid((flags & 0x80) != 0);
+            if (type != MSG_ALARM) {
 
-            if (type == MSG_ALARM) {
+                position.set(Position.KEY_ODOMETER, buf.readUnsignedMedium());
+                position.set(Position.KEY_STATUS, buf.readUnsignedInt());
+                position.set(Position.KEY_HDOP, buf.readUnsignedByte());
+                position.set(Position.KEY_VDOP, buf.readUnsignedByte());
+                position.set(Position.KEY_SATELLITES, buf.readUnsignedByte());
 
-                buf.skipBytes(2);
+                buf.skipBytes(5); // other location data
 
-            } else {
+                if (type == MSG_PERIPHERAL) {
 
-                position.set(Position.KEY_ODOMETER, buf.readUnsignedMedium());
+                    buf.readUnsignedShort(); // data length
 
-                buf.skipBytes(4); // status
-                buf.skipBytes(8); // other
+                    int dataType = buf.readUnsignedByte();
 
-            }
+                    buf.readUnsignedByte(); // content length
 
-            // parse extra data
+                    switch (dataType) {
+                        case 0x01:
+                            position.set(Position.KEY_FUEL,
+                                    buf.readUnsignedByte() * 100 + buf.readUnsignedByte());
+                            break;
+                        case 0x02:
+                            position.set(Position.PREFIX_TEMP + 1,
+                                    buf.readUnsignedByte() * 100 + buf.readUnsignedByte());
+                            break;
+                        default:
+                            break;
+                    }
+
+                }
+
+            }
 
             return position;