Fix GPS 056 frame decoder

2 files changed, 9 insertions, 1 deletions

diff --git a/src/org/traccar/protocol/Gps056FrameDecoder.java b/src/org/traccar/protocol/Gps056FrameDecoder.java
index 7adfddd07..4ce83dc0a 100644
--- a/src/org/traccar/protocol/Gps056FrameDecoder.java
+++ b/src/org/traccar/protocol/Gps056FrameDecoder.java
@@ -33,7 +33,11 @@ public class Gps056FrameDecoder extends FrameDecoder {
         if (buf.readableBytes() >= MESSAGE_HEADER) {
             int length = Integer.parseInt(buf.toString(2, 2, StandardCharsets.US_ASCII)) + 5;
             if (buf.readableBytes() >= length) {
-                return buf.readBytes(length);
+                ChannelBuffer frame = buf.readBytes(length);
+                while (buf.readable() && buf.getUnsignedByte(buf.readerIndex()) != '$') {
+                    buf.readByte();
+                }
+                return frame;
             }
         }
 
diff --git a/test/org/traccar/protocol/Gps056FrameDecoderTest.java b/test/org/traccar/protocol/Gps056FrameDecoderTest.java
index 548154b22..a5b5c5bbd 100644
--- a/test/org/traccar/protocol/Gps056FrameDecoderTest.java
+++ b/test/org/traccar/protocol/Gps056FrameDecoderTest.java
@@ -12,6 +12,10 @@ public class Gps056FrameDecoderTest extends ProtocolTest {
         Gps056FrameDecoder decoder = new Gps056FrameDecoder();
 
         Assert.assertEquals(
+                binary("242435314750534c5f30323836323436323033333738323934361905110f160b0b7710584e1cbd1b9b4500005b100300fb0a071700ffff23"),
+                decoder.decode(null, null, binary("242435314750534c5f30323836323436323033333738323934361905110f160b0b7710584e1cbd1b9b4500005b100300fb0a071700ffff230030")));
+
+        Assert.assertEquals(
                 binary("242432354c4f474e5f3131383632343632303333373832393436322e3123"),
                 decoder.decode(null, null, binary("242432354c4f474e5f3131383632343632303333373832393436322e3123")));