1 files changed, 37 insertions, 65 deletions

diff --git a/tracker-hatire/ftnoir_tracker_hat.cpp b/tracker-hatire/ftnoir_tracker_hat.cpp
index e8da968f..9948c30e 100644
--- a/tracker-hatire/ftnoir_tracker_hat.cpp
+++ b/tracker-hatire/ftnoir_tracker_hat.cpp
@@ -10,27 +10,20 @@
  */
 #include <QDebug>
 #include "ftnoir_tracker_hat.h"
+#include "compat/math.hpp"
 #include <algorithm>
 
 hatire::hatire()
 {
-        HAT.Rot[0]=0;
-        HAT.Rot[1]=0;
-        HAT.Rot[2]=0;
-        HAT.Trans[0]=0;
-        HAT.Trans[1]=0;
-        HAT.Trans[2]=0;
-
-        Begin.append((char) 0xAA);
-        Begin.append((char) 0xAA);
-        End.append((char) 0x55);
-        End.append((char) 0x55);
-}
 
-hatire::~hatire()
-{
+    Begin.append((unsigned char) 0xAA);
+    Begin.append((unsigned char) 0xAA);
+    End.append((unsigned char) 0x55);
+    End.append((unsigned char) 0x55);
 }
 
+hatire::~hatire() = default;
+
 //send RESET to Arduino
 void hatire::reset()
 {
@@ -43,27 +36,27 @@ void hatire::get_info( int *tps )
         *tps=frame_cnt;
         frame_cnt=0;
 }
-void hatire::start_tracker(QFrame*)
+module_status hatire::start_tracker(QFrame*)
 {
-        CptError=0;
-        frame_cnt=0;
+    CptError=0;
+    frame_cnt=0;
     t.Log("Starting Tracker");
 
     serial_result ret = t.init_serial_port();
 
+    t.start();
+
     switch (ret.code)
     {
     case result_ok:
-        break;
+        return status_ok();
     case result_error:
-        QMessageBox::warning(0, tr("Error"), ret.error, QMessageBox::Ok,QMessageBox::NoButton);
-        break;
+        return error(ret.error);
     case result_open_error:
-        QMessageBox::warning(0, tr("Error"), tr("Unable to open ComPort: %1").arg(ret.error), QMessageBox::Ok,QMessageBox::NoButton);
-        break;
+        return error(tr("Unable to open ComPort: %1").arg(ret.error));
+    default:
+        return error(tr("Unknown error"));
     }
-
-    t.start();
 }
 
 void hatire::serial_info()
@@ -106,70 +99,49 @@ 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;
-        }
+        qDebug() << "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.
+    for (unsigned k = 0; k < 3; k++)
+        HAT.Rot[k] = std::clamp(HAT.Rot[k], -180.f, 180.f);
 
     const struct
     {
+        double& place;
+        float input;
         bool enable;
         bool sign;
-        float input;
-        double& place;
     } spec[] =
     {
-        { s.EnableX, s.InvertX, HAT.Trans[s.XAxis], data[TX] },
-        { s.EnableY, s.InvertY, HAT.Trans[s.YAxis], data[TY] },
-        { s.EnableZ, s.InvertZ, HAT.Trans[s.ZAxis], data[TZ] },
-        { s.EnableYaw, s.InvertYaw, HAT.Rot[s.YawAxis], data[Yaw] },
-        { s.EnablePitch, s.InvertPitch, HAT.Rot[s.PitchAxis], data[Pitch] },
-        { s.EnableRoll, s.InvertRoll, HAT.Rot[s.RollAxis], data[Roll] },
+        { data[TX],    HAT.Trans[s.XAxis],   s.EnableX,     s.InvertX, },
+        { data[TY],    HAT.Trans[s.YAxis],   s.EnableY,     s.InvertY, },
+        { data[TZ],    HAT.Trans[s.ZAxis],   s.EnableZ,     s.InvertZ, },
+        { data[Yaw],   HAT.Rot[s.YawAxis],   s.EnableYaw,   s.InvertYaw },
+        { data[Pitch], HAT.Rot[s.PitchAxis], s.EnablePitch, s.InvertPitch },
+        { data[Roll],  HAT.Rot[s.RollAxis],  s.EnableRoll,  s.InvertRoll },
     };
 
-    for (unsigned i = 0; i < sizeof(spec) / sizeof(*spec); i++)
-    {
-        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;
+    for (auto& k : spec)
+        k.place = (k.sign ? -1 : 1) * (k.enable ? (double)k.input : 0);
 }
 
 #include "ftnoir_tracker_hat_dialog.h"