tracker/hatire: fix breakage

Issue: #139

3 files changed, 33 insertions, 54 deletions

diff --git a/tracker-hatire/ftnoir_tracker_hat.cpp b/tracker-hatire/ftnoir_tracker_hat.cpp
index e8da968f..30f40e60 100644
--- a/tracker-hatire/ftnoir_tracker_hat.cpp
+++ b/tracker-hatire/ftnoir_tracker_hat.cpp
@@ -106,44 +106,27 @@ void hatire::data(double *data)
 
                 if (ArduinoData.Code <= 1000)
                     HAT = ArduinoData;
-                else
-                    emit t.serial_debug_info(data_read.mid(4,24))  ;
+
                 data_read.remove(0, 30);
             }
             else
             {
+                CptError++;
                 // resync frame
-                int index =	data_read.indexOf(Begin, 1);
+                const int index = data_read.indexOf(Begin, 1);
                 if (index == -1)
-                {
-                    index = data_read.length();
-                }
-
-                if (data_read.length() != 0)
-                {
-                    emit t.serial_debug_info(data_read.mid(0,index));
-
+                    data_read.clear();
+                else
                     data_read.remove(0, index);
-
-                    CptError++;
-
-                    qDebug() << QTime::currentTime() << "hatire resync stream" << "index" << index << "remaining" << data_read.size();
-                }
             }
         }
     }
 
     if (CptError > 50)
     {
-        emit t.serial_debug_info("Can't find HAT frame");
-                CptError=0;
-        }
-
-        // Need to handle this differently in opentrack as opposed to tracknoir
-    //if  (new_frame) {
-        // in open track always populate the data, it seems opentrack always gives us a zeroed data structure to populate with pose data.
-        // if we have no new data, we don't populate it and so 0 pose gets handed back which is wrong. By always running the code below, if we
-        // have no new data, we will just give it the previous pose data which is the best thing we can do really.
+        qDebug() << "Can't find HAT frame";
+        CptError=0;
+    }
 
     const struct
     {
@@ -166,10 +149,6 @@ void hatire::data(double *data)
         auto& k = spec[i];
         k.place = (k.sign ? -1.f : 1.f) * (k.enable ? k.input : 0.f);
     }
-
-    // For debug
-    //data->x=dataRead.length();
-    //data->y=CptError;
 }
 
 #include "ftnoir_tracker_hat_dialog.h"
diff --git a/tracker-hatire/thread.cpp b/tracker-hatire/thread.cpp
index cc0f3012..506ccfcf 100644
--- a/tracker-hatire/thread.cpp
+++ b/tracker-hatire/thread.cpp
@@ -88,8 +88,6 @@ hatire_thread::~hatire_thread()
 
 hatire_thread::hatire_thread()
 {
-    data_read.reserve(65536);
-
     connect(this, &QThread::finished, this, &hatire_thread::teardown_serial, Qt::DirectConnection);
     connect(this, &hatire_thread::init_serial_port, this, &hatire_thread::init_serial_port_impl, Qt::QueuedConnection);
     connect(this, &hatire_thread::serial_info, this, &hatire_thread::serial_info_impl, Qt::QueuedConnection);
@@ -135,8 +133,8 @@ void hatire_thread::run()
     com_port.setFileName(HATIRE_DEBUG_LOGFILE);
     com_port.open(QIODevice::ReadOnly);
 
-    read_timer.start(10);
     connect(&read_timer, &QTimer::timeout, this, &hatire_thread::on_serial_read, Qt::DirectConnection);
+    read_timer.start(5);
 #endif
     (void) exec();
 
@@ -302,38 +300,38 @@ void hatire_thread::serial_info_impl()
 
 void hatire_thread::on_serial_read()
 {
+    const int sz = com_port.read(buf, sizeof(buf));
+
+    if (sz > 0)
     {
+        stat.input(timer.elapsed_ms());
+        timer.start();
+
         QMutexLocker lck(&data_mtx);
-#ifndef HATIRE_DEBUG_LOGFILE
-        data_read += com_port.readAll();
-#else
-        QByteArray tmp = com_port.read(30);
-        data_read += tmp;
-        if (tmp.length() == 0 && read_timer.isActive())
-        {
-            qDebug() << "eof";
-            read_timer.stop();
-        }
-#endif
+        data_read.append(buf, sz);
     }
-
-    stat.input(timer.elapsed_ms());
-    timer.start();
-
-    if (throttle_timer.elapsed_ms() >= 3000)
+#if defined HATIRE_DEBUG_LOGFILE
+    else
     {
-        throttle_timer.start();
-        qDebug() << "stat:" << "avg" << stat.avg() << "stddev" << stat.stddev();
+        qDebug() << "eof";
+        read_timer.stop();
     }
+#endif
 
-    if (s.serial_bug_workaround)
+    if (s.serial_bug_workaround || sz <= 0)
     {
         // qt can fire QSerialPort::readyRead() needlessly, causing a busy loop.
         // see https://github.com/opentrack/opentrack/issues/327#issuecomment-207941003
-        constexpr int hz = 90;
-        constexpr int ms = 1000/hz;
+        static constexpr int hz = 90;
+        static constexpr int ms = 1000/hz;
         portable::sleep(ms);
     }
+
+    if (throttle_timer.elapsed_ms() >= 3000)
+    {
+        throttle_timer.start();
+        qDebug() << "stat:" << "avg" << stat.avg() << "stddev" << stat.stddev();
+    }
 }
 
 QByteArray& hatire_thread::send_data_read_nolock()
diff --git a/tracker-hatire/thread.hpp b/tracker-hatire/thread.hpp
index 962b902b..93c50c9e 100644
--- a/tracker-hatire/thread.hpp
+++ b/tracker-hatire/thread.hpp
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "ftnoir_arduino_type.h"
 #include "ftnoir_tracker_hat_settings.h"
 
 #include <QSerialPort>
@@ -20,7 +21,7 @@ enum results
     result_error,
 };
 
-//#define HATIRE_DEBUG_LOGFILE "d:/putty-hatire.log"
+//#define HATIRE_DEBUG_LOGFILE "c:/users/sthalik/test-random"
 
 #ifdef HATIRE_DEBUG_LOGFILE
 #   include <QFile>
@@ -52,6 +53,7 @@ class hatire_thread : public QThread
     TrackerSettings s;
     variance stat;
     Timer timer, throttle_timer;
+    char buf[1024];
 
     void run() override;
     static inline QByteArray to_latin1(const QString& str) { return str.toLatin1(); }