3 files changed, 62 insertions, 43 deletions

diff --git a/tracker-hatire/ftnoir_tracker_hat.cpp b/tracker-hatire/ftnoir_tracker_hat.cpp
index 879ea618..08313be0 100755
--- a/tracker-hatire/ftnoir_tracker_hat.cpp
+++ b/tracker-hatire/ftnoir_tracker_hat.cpp
@@ -88,46 +88,62 @@ void hatire::data(double *data)
     {
         QMutexLocker l(&t.data_mtx);
 
-        QByteArray dataRead(t.flush_data_read_nolock());
+        bool ok = false;
 
-        while (dataRead.length() >= 30)
+        QByteArray& data_read = t.send_data_read_nolock(ok);
+
+        if (ok)
         {
-            t.Log(dataRead.toHex());
-            if (dataRead.startsWith(Begin) && dataRead.mid(28,2) == End)
-            { // .Begin==0xAAAA .End==0x5555
-                QDataStream datastream(dataRead.left(30));
-                if (ts.bBigEndian)	datastream.setByteOrder(QDataStream::BigEndian );
-                else datastream.setByteOrder(QDataStream::LittleEndian);
-                datastream>>ArduinoData;
-                frame_cnt++;
-                if (ArduinoData.Code <= 1000)
+
+            while (data_read.length() >= 30)
+            {
+                //t.Log(data_read.toHex());
+                // .Begin==0xAAAA .End==0x5555
+                if (data_read[0] == Begin[0] && data_read[1] == Begin[1] &&
+                    data_read[28] == End[0] && data_read[29] == End[1])
                 {
-                    HAT=ArduinoData;
-                    new_frame=true;
+                    QDataStream stream(&data_read, QIODevice::ReadOnly);
+
+                    if (ts.bBigEndian)
+                        stream.setByteOrder(QDataStream::BigEndian);
+                    else
+                        stream.setByteOrder(QDataStream::LittleEndian);
+
+                    stream >> ArduinoData;
+
+                    frame_cnt++;
+
+                    if (ArduinoData.Code <= 1000)
+                        HAT = ArduinoData;
+                    else
+                        emit t.serial_debug_info(data_read.mid(4,24))  ;
+                    data_read.remove(0, 30);
                 }
                 else
                 {
-                    emit t.serial_debug_info(dataRead.mid(4,24))  ;
-                }
-                dataRead.remove(0,30);
-            }
-            else
-            {
-                bool ok = true;
-                // resynchro trame
-                int index =	dataRead.indexOf(Begin);
-                if (index==-1) {
-                    ok = false;
-                    index=dataRead.length();
+                    bool ok = true;
+                    // resync frame
+                    int index =	data_read.indexOf(Begin);
+                    if (index == -1)
+                    {
+                        ok = false;
+                        index = data_read.length();
+                    }
+                    emit t.serial_debug_info(data_read.mid(0,index));
+
+                    if (!ok)
+                        data_read.clear();
+                    else
+                        data_read.remove(0, index);
+
+                    CptError++;
+
+                    qDebug() << QTime::currentTime() << "hatire resync stream" << "index" << index << "ok" << ok;
                 }
-                emit t.serial_debug_info(dataRead.mid(0,index))  ;
-                dataRead.remove(0,index);
-                CptError++;
-                qDebug() << QTime::currentTime() << "hatire resync stream" << "index" << index << "ok" << ok;
             }
-        }
 
-        t.prepend_unread_data_nolock(dataRead);
+            t.replace_data_nolock(std::move(data_read));
+        }
     }
 
     if (CptError > 50)
diff --git a/tracker-hatire/thread.cpp b/tracker-hatire/thread.cpp
index f8064896..a33d58e0 100755
--- a/tracker-hatire/thread.cpp
+++ b/tracker-hatire/thread.cpp
@@ -254,7 +254,8 @@ void hatire_thread::serial_info_impl()
 
 void hatire_thread::on_serial_read()
 {
-    static constexpr int ms = 1000/200;
+    constexpr int hz = 90;
+    constexpr int ms = 1000/hz;
 
     {
         QMutexLocker lck(&data_mtx);
@@ -275,23 +276,24 @@ void hatire_thread::on_serial_read()
     portable::sleep(ms);
 }
 
-void hatire_thread::prepend_unread_data_nolock(const QByteArray &data)
+void hatire_thread::replace_data_nolock(QByteArray&& data)
 {
-    data_read.prepend(data);
+    data_read = std::move(data);
 }
 
-QByteArray hatire_thread::flush_data_read_nolock()
+QByteArray& hatire_thread::send_data_read_nolock(bool& ret)
 {
     constexpr int packet_len = 30;
+    constexpr int cnt = 4;
+    constexpr int len = cnt * packet_len;
 
-    if (data_read.length() < 4 * packet_len)
+    if (data_read.length() < len)
     {
         // we're requesting more than packet length to help resync the stream if needed
-        return QByteArray();
+        ret = false;
     }
+    else
+        ret = true;
 
-    QByteArray ret = data_read.left(90);
-    data_read = data_read.mid(90);
-
-    return ret;
+    return data_read;
 }
diff --git a/tracker-hatire/thread.hpp b/tracker-hatire/thread.hpp
index c4ce4761..b1861bac 100755
--- a/tracker-hatire/thread.hpp
+++ b/tracker-hatire/thread.hpp
@@ -165,9 +165,10 @@ public:
     ~hatire_thread() override;
     hatire_thread();
 
-    void prepend_unread_data_nolock(const QByteArray& data);
+    void replace_data_nolock(QByteArray&& data);
+
+    QByteArray& send_data_read_nolock(bool& ret);
 
-    QByteArray flush_data_read_nolock();
     void Log(const QString& message);
 
     QMutex data_mtx;