summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2016-04-29 11:30:37 +0200
committerStanislaw Halik <sthalik@misaki.pl>2016-04-29 11:30:37 +0200
commit38dd6e55d20adfd830d834c394fc6ce7373a4805 (patch)
tree0a94bf051ff0bc153abebaa74e474748c2d4d8a6
parent4db6f6334d13ed5e8696dfa0208b42b3e9a2352a (diff)
tracker/hatire: move io to a separate thread
We can't have async io on the main thread because QSerialPort's readyRead() signal can fire constantly, thus consuming all CPU time. We can't sleep in the main thread either as that blocks too many things. We can't ignore readyRead() invocations over a threshold as that'll make us lose some of data notifications. Refactor hatire to put IO on a thread. Since this is a separate Qt event loop, we may sleep in there. Further, add a debug mode reading data from a file, as if it came from a serial-attached device. Issue: #327
-rw-r--r--tracker-hatire/ftnoir_arduino_type.h8
-rw-r--r--tracker-hatire/ftnoir_tracker_hat.cpp535
-rw-r--r--tracker-hatire/ftnoir_tracker_hat.h122
-rw-r--r--tracker-hatire/ftnoir_tracker_hat_dialog.cpp41
-rw-r--r--tracker-hatire/ftnoir_tracker_hat_dialog.h26
-rw-r--r--tracker-hatire/ftnoir_tracker_hat_settings.cpp22
-rw-r--r--tracker-hatire/ftnoir_tracker_hat_settings.h10
-rw-r--r--tracker-hatire/thread.cpp300
-rw-r--r--tracker-hatire/thread.hpp167
9 files changed, 590 insertions, 641 deletions
diff --git a/tracker-hatire/ftnoir_arduino_type.h b/tracker-hatire/ftnoir_arduino_type.h
index 57afed63..481e137b 100644
--- a/tracker-hatire/ftnoir_arduino_type.h
+++ b/tracker-hatire/ftnoir_arduino_type.h
@@ -1,5 +1,4 @@
-#ifndef FTNOIR_TARDUINO_TYPE_H
-#define FTNOIR_TARDUINO_TYPE_H
+#pragma once
#include <QDataStream>
@@ -15,7 +14,6 @@ struct TArduinoData
} ;
#pragma pack(pop)
-
inline QDataStream & operator >> ( QDataStream& in, TArduinoData& out )
{
in.setFloatingPointPrecision(QDataStream::SinglePrecision );
@@ -27,6 +25,4 @@ inline QDataStream & operator >> ( QDataStream& in, TArduinoData& out )
return in;
}
-
-
-#endif
+static_assert(sizeof(TArduinoData) == 30, "sizeof packet != 30");
diff --git a/tracker-hatire/ftnoir_tracker_hat.cpp b/tracker-hatire/ftnoir_tracker_hat.cpp
index eea1cbca..6b76ddba 100644
--- a/tracker-hatire/ftnoir_tracker_hat.cpp
+++ b/tracker-hatire/ftnoir_tracker_hat.cpp
@@ -11,12 +11,8 @@
#include <QDebug>
#include "ftnoir_tracker_hat.h"
-FTNoIR_Tracker::FTNoIR_Tracker()
+hatire::hatire()
{
- qDebug()<<"Tracker::HAT";
-
- ComPort = NULL;
-
HAT.Rot[0]=0;
HAT.Rot[1]=0;
HAT.Rot[2]=0;
@@ -24,415 +20,126 @@ FTNoIR_Tracker::FTNoIR_Tracker()
HAT.Trans[1]=0;
HAT.Trans[2]=0;
-
- // prepare & reserve QByteArray
- dataRead.resize(4096);
- dataRead.clear();
Begin.append((char) 0xAA);
Begin.append((char) 0xAA);
End.append((char) 0x55);
End.append((char) 0x55);
- flDiagnostics.setFileName(QCoreApplication::applicationDirPath() + "/HATDiagnostics.txt");
-
settings.load_ini();
}
-FTNoIR_Tracker::~FTNoIR_Tracker()
+hatire::~hatire()
{
- qDebug()<<"Tracker::~HAT";
- if (ComPort!=NULL) {
- if (ComPort->isOpen() ) {
-
-#ifdef OPENTRACK_API
- QByteArray Msg;
- Log("Tracker shut down");
- ComPort->write(sCmdStop);
- if (!ComPort->waitForBytesWritten(1000)) {
- emit sendMsgInfo("TimeOut in writing CMD");
- } else
- {
- Msg.append("\r\n");
- Msg.append("SEND '");
- Msg.append(sCmdStop);
- Msg.append("'\r\n");
- }
- emit sendMsgInfo(Msg);
-#endif
- ComPort->close();
- disconnect(ComPort,SIGNAL(readyRead()),0,0);
-
- }
- delete ComPort;
- ComPort=NULL;
- }
-}
-
-
-//send ZERO to Arduino
-bool FTNoIR_Tracker::notifyZeroed() {
- qDebug() << " HAT send ZEROed ";
- sendcmd(sCmdZero);
- return true;
}
-
-
//send RESET to Arduino
-void FTNoIR_Tracker::reset() {
- qDebug() << " HAT send RESET ";
- sendcmd(sCmdReset);
-}
-
-
-// Info SerialPort
-void FTNoIR_Tracker::SerialInfo() {
- QByteArray Msg;
- if (ComPort!=NULL) {
- if (ComPort->isOpen() ) {
- Msg.append("\r\n");
- Msg.append(ComPort->portName());
- Msg.append("\r\n");
- Msg.append("BAUDRATE :");
- Msg.append(QString::number(ComPort->baudRate()));
- Msg.append("\r\n");
- Msg.append("DataBits :");
- Msg.append(QString::number(ComPort->dataBits()));
- Msg.append("\r\n");
- Msg.append("Parity :");
- switch (ComPort->parity()) {
- case 0: Msg.append("No parity");
- break;
- case 2: Msg.append("Even parity");
- break;
- case 3: Msg.append("Odd parity");
- break;
- case 4: Msg.append("Space parity");
- break;
- case 5: Msg.append("Mark parity");
- break;
- default: Msg.append("Unknown parity");
- break;
- }
- Msg.append("\r\n");
- Msg.append("Stop Bits :");
- switch (ComPort->stopBits()) {
- Msg.append(QString::number(ComPort->stopBits()));
- case 1: Msg.append("1 stop bit.");
- break;
- case 2: Msg.append("2 stop bits.");
- break;
- case 3: Msg.append("1.5 stop bits.");
- break;
- default: Msg.append("Unknown number of stop bit.");
- break;
- }
- Msg.append("\r\n");
- Msg.append("Flow Control :");
- switch (ComPort->flowControl()) {
- case 0: Msg.append("No flow control");
- break;
- case 1: Msg.append("Hardware flow control (RTS/CTS)");
- break;
- case 2: Msg.append("Software flow control (XON/XOFF)");
- break;
- default: Msg.append("Unknown flow control");
- break;
- }
- emit sendMsgInfo(Msg);
-
- }
- }
-}
-
-
-//send command to Arduino
-void FTNoIR_Tracker::sendcmd(const QByteArray &cmd) {
- QByteArray Msg;
- if (cmd.length()>0) {
- if (ComPort->isOpen() )
- {
- QString logMess;
- logMess.append("SEND '");
- logMess.append(cmd);
- logMess.append("'");
- Log(logMess);
- ComPort->write(cmd);
- if (!ComPort->waitForBytesWritten(1000)) {
- emit sendMsgInfo("TimeOut in writing CMD");
- } else
- {
- Msg.append("\r\n");
- Msg.append("SEND '");
- Msg.append(cmd);
- Msg.append("'\r\n");
- }
- #if 0 // WaitForReadyRead isn't working well and there are some reports of it being a win32 issue. We can live without it anyway
- if ( !ComPort->waitForReadyRead(1000)) {
- emit sendMsgInfo("TimeOut in response to CMD") ;
- } else {
- emit sendMsgInfo(Msg);
- }
- #else
- emit sendMsgInfo(Msg);
- #endif
- } else {
- emit sendMsgInfo("ComPort not open") ;
- }
- }
+void hatire::reset()
+{
+ t.sendcmd(ts.sCmdReset);
}
-
// return FPS
-void FTNoIR_Tracker::get_info( int *tps ){
+void hatire::get_info( int *tps )
+{
*tps=frame_cnt;
frame_cnt=0;
}
-
-void FTNoIR_Tracker::SerialRead()
-{
- QMutexLocker lck(&mutex);
- dataRead+=ComPort->readAll();
-}
-
-#ifndef OPENTRACK_API
-void FTNoIR_Tracker::Initialize( QFrame *videoframe )
+void hatire::start_tracker(QFrame*)
{
CptError=0;
- dataRead.clear();
frame_cnt=0;
-
- Log("INITIALISING HATIRE");
-
+ new_frame=false;
settings.load_ini();
applysettings(settings);
- ComPort = new QSerialPort(this);
- ComPort->setPortName(sSerialPortName);
- if (ComPort->open(QIODevice::ReadWrite ) == true) {
- connect(ComPort, SIGNAL(readyRead()), this, SLOT(SerialRead()));
- if (
- ComPort->setBaudRate((QSerialPort::BaudRate)iBaudRate)
- && ComPort->setDataBits((QSerialPort::DataBits)iDataBits)
- && ComPort->setParity((QSerialPort::Parity)iParity)
- && ComPort->setStopBits((QSerialPort::StopBits)iStopBits)
- && ComPort->setFlowControl((QSerialPort::FlowControl)iFlowControl)
- && ComPort->clear(QSerialPort::AllDirections)
- && ComPort->setDataErrorPolicy(QSerialPort::IgnorePolicy)
- ) {
- // Wait init arduino sequence
- for (int i = 1; i <=iDelayInit; i+=50) {
- if (ComPort->waitForReadyRead(50)) break;
- }
- sendcmd(sCmdInit);
- // Wait init MPU sequence
- for (int i = 1; i <=iDelayStart; i+=50) {
- if (ComPort->waitForReadyRead(50)) break;
- }
-
- } else {
- QMessageBox::warning(0,"Error", ComPort->errorString(),QMessageBox::Ok,QMessageBox::NoButton);
- }
- }
- else {
- QMessageBox::warning(0,"Error", "Unable to open ComPort",QMessageBox::Ok,QMessageBox::NoButton);
- delete ComPort;
- ComPort = NULL;
- }
- return;
-}
-
-
+ t.Log("Starting Tracker");
+
+ serial_result ret = t.init_serial_port();
+
+ switch (ret.code)
+ {
+ case result_ok:
+ break;
+ case result_error:
+ QMessageBox::warning(0,"Error", ret.error, QMessageBox::Ok,QMessageBox::NoButton);
+ break;
+ case result_open_error:
+ QMessageBox::warning(0,"Error", "Unable to open ComPort: " + ret.error, QMessageBox::Ok,QMessageBox::NoButton);
+ break;
+ }
-void FTNoIR_Tracker::StartTracker(HWND parent_window)
-{
- // Send START cmd to IMU
- sendcmd(sCmdStart);
- Log("Starting Tracker");
- // Wait start MPU sequence
- for (int i = 1; i <=iDelaySeq; i+=50) {
- if (ComPort->waitForReadyRead(50)) break;
- }
- return;
+ t.start(ts);
}
-
-void FTNoIR_Tracker::StopTracker( bool exit )
+void hatire::serial_info()
{
- QByteArray Msg;
-
- Log("Stopping tracker");
- if (sCmdStop.length()>0) {
- if (ComPort->isOpen() )
- {
- ComPort->write(sCmdStop);
- if (!ComPort->waitForBytesWritten(1000)) {
- emit sendMsgInfo("TimeOut in writing CMD");
- } else
- {
- Msg.append("\r\n");
- Msg.append("SEND '");
- Msg.append(sCmdStop);
- Msg.append("'\r\n");
- }
- emit sendMsgInfo(Msg);
- }
- }
- // OK, the thread is not stopped, doing this. That might be dangerous anyway...
- //
- if (exit || !exit) return;
- return;
-}
-//send CENTER to Arduino
-void FTNoIR_Tracker::notifyCenter() {
- sendcmd(sCmdCenter);
+ t.serial_info();
}
-
-#else
-void FTNoIR_Tracker::start_tracker(QFrame*)
+void hatire::send_serial_command(const QByteArray& x)
{
- CptError=0;
- dataRead.clear();
- frame_cnt=0;
- new_frame=false;
- settings.load_ini();
- applysettings(settings);
- ComPort = new QSerialPort(this);
- ComPort->setPortName(sSerialPortName);
- Log("Starting Tracker");
-
- if (ComPort->open(QIODevice::ReadWrite ) == true) {
- connect(ComPort, SIGNAL(readyRead()), this, SLOT(SerialRead()));
- Log("Port Open");
- if (
- ComPort->setBaudRate((QSerialPort::BaudRate)iBaudRate)
- && ComPort->setDataBits((QSerialPort::DataBits)iDataBits)
- && ComPort->setParity((QSerialPort::Parity)iParity)
- && ComPort->setStopBits((QSerialPort::StopBits)iStopBits)
- && ComPort->setFlowControl((QSerialPort::FlowControl)iFlowControl)
- && ComPort->clear(QSerialPort::AllDirections)
- && ComPort->setDataErrorPolicy(QSerialPort::IgnorePolicy)
- ) {
- Log("Port Parameters set");
- qDebug() << QTime::currentTime() << " HAT OPEN on " << ComPort->portName() << ComPort->baudRate() << ComPort->dataBits() << ComPort->parity() << ComPort->stopBits() << ComPort->flowControl();
-
- if (ComPort->flowControl() == QSerialPort::HardwareControl)
- {
- // Raise DTR
- Log("Raising DTR");
- if (!ComPort->setDataTerminalReady(true))
- Log("Couldn't set DTR");
-
- // Raise RTS/CTS
- Log("Raising RTS");
- if (!ComPort->setRequestToSend(true))
- Log("Couldn't set RTS");
-
- }
- // Wait init arduino sequence
- for (int i = 1; i <=iDelayInit; i+=50) {
- if (ComPort->waitForReadyRead(50)) break;
- }
- Log("Waiting on init");
- qDebug() << QTime::currentTime() << " HAT send INIT ";
- sendcmd(sCmdInit);
- // Wait init MPU sequence
- for (int i = 1; i <=iDelayStart; i+=50) {
- if (ComPort->waitForReadyRead(50)) break;
- }
- // Send START cmd to IMU
- qDebug() << QTime::currentTime() << " HAT send START ";
- sendcmd(sCmdStart);
-
- // Wait start MPU sequence
- for (int i = 1; i <=iDelaySeq; i+=50) {
- if (ComPort->waitForReadyRead(50)) break;
- }
- Log("Port setup, waiting for HAT frames to process");
- qDebug() << QTime::currentTime() << " HAT wait MPU ";
- } else {
- QMessageBox::warning(0,"Error", ComPort->errorString(),QMessageBox::Ok,QMessageBox::NoButton);
- }
- }
- else {
- QMessageBox::warning(0,"Error", "Unable to open ComPort: " + ComPort->errorString(), QMessageBox::Ok,QMessageBox::NoButton);
- delete ComPort;
- ComPort = NULL;
- }
- return;
-
+ t.sendcmd(x);
}
-//send CENTER to Arduino
-void FTNoIR_Tracker::center() {
- qDebug() << " HAT send CENTER ";
- Log("Sending Centre Command");
-
- sendcmd(sCmdCenter);
-}
-
-//Return speed FPS sketch Arduino
-int FTNoIR_Tracker::preferredHz() {
- qDebug() << " HAT return Preferred FPS " << iFpsArduino;
- return iFpsArduino;
-}
-
-#endif
-
-
//
// Return 6DOF info
//
-#ifdef OPENTRACK_API
-void FTNoIR_Tracker::data(double *data)
-#else
-bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data)
-#endif
+void hatire::data(double *data)
{
- QMutexLocker lck(&mutex);
- while (dataRead.length()>=30) {
- Log(dataRead.toHex());
- if ((dataRead.startsWith(Begin) && ( dataRead.mid(28,2)==End )) ) { // .Begin==0xAAAA .End==0x5555
- QDataStream datastream(dataRead.left(30));
- if (bBigEndian) datastream.setByteOrder(QDataStream::BigEndian );
- else datastream.setByteOrder(QDataStream::LittleEndian );
+ QByteArray dataRead(t.flush_data_read());
+
+ while (dataRead.length() >= 30)
+ {
+ 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) {
+ if (ArduinoData.Code <= 1000)
+ {
HAT=ArduinoData;
new_frame=true;
- } else {
- emit sendMsgInfo(dataRead.mid(4,24)) ;
+ }
+ else
+ {
+ emit t.serial_debug_info(dataRead.mid(4,24)) ;
}
dataRead.remove(0,30);
- } else {
+ }
+ else
+ {
+ bool ok = true;
// resynchro trame
int index = dataRead.indexOf(Begin);
if (index==-1) {
+ ok = false;
index=dataRead.length();
}
- emit sendMsgInfo(dataRead.mid(0,index)) ;
+ emit t.serial_debug_info(dataRead.mid(0,index)) ;
dataRead.remove(0,index);
CptError++;
- qDebug() << QTime::currentTime() << " HAT Resync-Frame, counter " << CptError;
+ qDebug() << QTime::currentTime() << "hatire resync stream" << "index" << index << "ok" << ok;
}
}
- if (CptError>50) {
- emit sendMsgInfo("Can't find HAT frame") ;
+ t.prepend_unread_data(dataRead);
+
+ if (CptError > 50)
+ {
+ emit t.serial_debug_info("Can't find HAT frame");
CptError=0;
-#ifndef OPENTRACK_API
- return false;
-#endif
}
+
+ // XXX fix copy-pasted code -sh 20160410
+
// Need to handle this differently in opentrack as opposed to tracknoir
//if (new_frame) {
-#ifdef OPENTRACK_API
// 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.
- if(1){
-
+
if (bEnableYaw) {
if (bInvertYaw ) data[Yaw] = HAT.Rot[iYawAxe] * -1.0f;
else data[Yaw] = HAT.Rot[iYawAxe];
@@ -463,54 +170,20 @@ bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data)
if (bInvertZ) data[TZ] = HAT.Trans[iZAxe]* -1.0f;
else data[TZ] = HAT.Trans[iZAxe];
} else data[TZ] =0;
-#else
- if (new_frame) { // treat frame handling as it was for TrackNoIR.
- if (bEnableYaw) {
- if (bInvertYaw ) data->yaw = (double) HAT.Rot[iYawAxe] * -1.0f;
- else data->yaw = (double) HAT.Rot[iYawAxe];
- }
-
- if (bEnablePitch) {
- if (bInvertPitch)data->pitch = (double) HAT.Rot[iPitchAxe] * -1.0f;
- else data->pitch = (double) HAT.Rot[iPitchAxe];
- }
- if (bEnableRoll) {
- if (bInvertRoll) data->roll = (double) HAT.Rot[iRollAxe] * -1.0f;
- else data->roll = (double) HAT.Rot[iRollAxe];
- }
-
- if (bEnableX) {
- if (bInvertX) data->x = (double) HAT.Trans[iXAxe]* -1.0f;
- else data->x = (double) HAT.Trans[iXAxe];
- }
-
- if (bEnableY) {
- if (bInvertY) data->y = (double) HAT.Trans[iYAxe]* -1.0f;
- else data->y = (double) HAT.Trans[iYAxe];
- }
+ new_frame=false;
- if (bEnableZ) {
- if (bInvertZ) data->z = (double) HAT.Trans[iZAxe]* -1.0f;
- else data->z = (double) HAT.Trans[iZAxe];
- }
- return true;
-#endif
- new_frame=false;
// For debug
//data->x=dataRead.length();
//data->y=CptError;
- }
}
-
-
//
// Apply modification Settings
//
-void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){
- QMutexLocker lck(&mutex);
- sSerialPortName= settings.SerialPortName;
+void hatire::applysettings(const TrackerSettings& settings)
+{
+ ts.sSerialPortName = settings.SerialPortName;
bEnableRoll = settings.EnableRoll;
bEnablePitch = settings.EnablePitch;
@@ -525,7 +198,7 @@ void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){
bInvertX = settings.InvertX;
bInvertY = settings.InvertY;
bInvertZ = settings.InvertZ;
- bEnableLogging = settings.EnableLogging;
+ ts.bEnableLogging = settings.EnableLogging;
iRollAxe= settings.RollAxe;
iPitchAxe= settings.PitchAxe;
@@ -534,64 +207,26 @@ void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){
iYAxe= settings.YAxe;
iZAxe= settings.ZAxe;
- iBaudRate=settings.pBaudRate;
- iDataBits=settings.pDataBits;
- iParity=settings.pParity;
- iStopBits=settings.pStopBits;
- iFlowControl=settings.pFlowControl;
-
- sCmdStart= settings.CmdStart.toLatin1();
- sCmdStop= settings.CmdStop.toLatin1();
- sCmdInit= settings.CmdInit.toLatin1();
- sCmdReset= settings.CmdReset.toLatin1();
- sCmdCenter= settings.CmdCenter.toLatin1();
- sCmdZero= settings.CmdZero.toLatin1();
-
- iDelayInit=settings.DelayInit;
- iDelayStart=settings.DelayStart;
- iDelaySeq=settings.DelaySeq;
-
- bBigEndian=settings.BigEndian;
-#ifdef OPENTRACK_API
- iFpsArduino=settings.FPSArduino;
-#endif
-}
-
-void FTNoIR_Tracker::Log(QString message)
-{
- // Drop out immediately if logging is off. Yes, there is still some overhead because of passing strings around for no reason.
- // that's unfortunate and I'll monitor the impact and see if it needs a more involved fix.
- if (!bEnableLogging) return;
- QString logMessage;
-
- if (flDiagnostics.open(QIODevice::ReadWrite | QIODevice::Append))
- {
- QTextStream out(&flDiagnostics);
- QString milliSeconds;
- milliSeconds = QString("%1").arg(QTime::currentTime().msec(), 3, 10, QChar('0'));
- // We have a file
- out << QTime::currentTime().toString() << "." << milliSeconds << ": " << message << "\r\n";
- flDiagnostics.close();
- }
-}
+ ts.iBaudRate=settings.pBaudRate;
+ ts.iDataBits=settings.pDataBits;
+ ts.iParity=settings.pParity;
+ ts.iStopBits=settings.pStopBits;
+ ts.iFlowControl=settings.pFlowControl;
+ ts.sCmdStart= settings.CmdStart.toLatin1();
+ ts.sCmdStop= settings.CmdStop.toLatin1();
+ ts.sCmdInit= settings.CmdInit.toLatin1();
+ ts.sCmdReset= settings.CmdReset.toLatin1();
+ ts.sCmdCenter= settings.CmdCenter.toLatin1();
+ ts.sCmdZero= settings.CmdZero.toLatin1();
+ ts.iDelayInit=settings.DelayInit;
+ ts.iDelayStart=settings.DelayStart;
+ ts.iDelaySeq=settings.DelaySeq;
+ ts.bBigEndian=settings.BigEndian;
-////////////////////////////////////////////////////////////////////////////////
-// Factory function that creates instances if the Tracker object.
+ t.update_serial_settings(ts);
+}
-// Export both decorated and undecorated names.
-// GetTracker - Undecorated name, which can be easily used with GetProcAddress
-// Win32 API function.
-// _GetTracker@0 - Common name decoration for __stdcall functions in C language.
-////////////////////////////////////////////////////////////////////////////////
-#ifdef OPENTRACK_API
#include "ftnoir_tracker_hat_dialog.h"
-OPENTRACK_DECLARE_TRACKER(FTNoIR_Tracker, TrackerControls, TrackerDll)
-#else
-#pragma comment(linker, "/export:GetTracker=_GetTracker@0")
-FTNOIR_TRACKER_BASE_EXPORT ITrackerPtr __stdcall GetTracker()
-{
- return new FTNoIR_Tracker;
-}
-#endif
+OPENTRACK_DECLARE_TRACKER(hatire, TrackerControls, TrackerDll)
diff --git a/tracker-hatire/ftnoir_tracker_hat.h b/tracker-hatire/ftnoir_tracker_hat.h
index 2472428b..4767d291 100644
--- a/tracker-hatire/ftnoir_tracker_hat.h
+++ b/tracker-hatire/ftnoir_tracker_hat.h
@@ -1,13 +1,10 @@
-#ifndef FTNOIR_TRACKER_HAT_H
-#define FTNOIR_TRACKER_HAT_H
+#pragma once
-#ifdef OPENTRACK_API
-# include "opentrack/plugin-support.hpp"
-#else
-# include "..\ftnoir_tracker_base\ftnoir_tracker_base.h"
-#endif
+#include "thread.hpp"
+#include "opentrack/plugin-support.hpp"
#include "ftnoir_tracker_hat_settings.h"
#include "ftnoir_arduino_type.h"
+
#include <QObject>
#include <QPalette>
#include <QtGui>
@@ -15,59 +12,42 @@
#include <QMessageBox>
#include <QtSerialPort/QSerialPort>
#include <QtSerialPort/QSerialPortInfo>
-#include <QMutex>
-#include <QMutexLocker>
#include <QSettings>
-#define VER_FILEVERSION_STR "Version 2.1.1\0"
+#define VER_FILEVERSION_STR "Version 2.1.1"
-class FTNoIR_Tracker : public QObject, public ITracker
+class hatire : public QObject, public ITracker
{
Q_OBJECT
+
public:
- FTNoIR_Tracker();
- ~FTNoIR_Tracker();
+ hatire();
+ ~hatire();
-#ifdef OPENTRACK_API
void start_tracker(QFrame*);
void data(double *data);
- int preferredHz(); // unused
- void center(); // unused
-#else
- void Initialize( QFrame *videoframe );
- void StartTracker(HWND parent_window);
- void StopTracker(bool exit);
- bool GiveHeadPoseData(THeadPoseData *data);
- void notifyCenter();
-#endif
+ //void center();
void applysettings(const TrackerSettings& settings);
- bool notifyZeroed();
+ //bool notifyZeroed();
void reset();
- void SerialInfo();
- void sendcmd(const QByteArray &cmd);
void get_info( int *tps );
-
-private Q_SLOTS:
- void SerialRead();
- void Log(QString message);
-
-signals:
- void sendMsgInfo(const QByteArray &MsgInfo);
-
+ void serial_info();
+ void send_serial_command(const QByteArray& x);
private:
- QSerialPort *ComPort;
- TArduinoData ArduinoData, HAT ; // Trame from Arduino
- QByteArray dataRead;
- QByteArray dataToSend;
+ TArduinoData ArduinoData, HAT;
QByteArray Begin;
QByteArray End;
- QMutex mutex;
- int frame_cnt;
- bool new_frame;
+ hatire_thread t;
+ thread_settings ts;
+
+ // XXX move to settings api -sh 20160410
TrackerSettings settings;
+ int frame_cnt;
+ bool new_frame;
+
bool bEnableRoll;
bool bEnablePitch;
bool bEnableYaw;
@@ -81,7 +61,6 @@ private:
bool bInvertX;
bool bInvertY;
bool bInvertZ;
- bool bEnableLogging;
int iRollAxe;
int iPitchAxe;
@@ -90,68 +69,11 @@ private:
int iYAxe;
int iZAxe;
- QByteArray sCmdStart;
- QByteArray sCmdStop;
- QByteArray sCmdInit;
- QByteArray sCmdReset;
- QByteArray sCmdCenter;
- QByteArray sCmdZero;
-
- int iDelayInit;
- int iDelayStart;
- int iDelaySeq;
-
- bool bBigEndian;
-
- QString sSerialPortName;
- QSerialPort::BaudRate iBaudRate;
- QSerialPort::DataBits iDataBits;
- QSerialPort::Parity iParity;
- QSerialPort::StopBits iStopBits;
- QSerialPort::FlowControl iFlowControl;
-
- QFile flDiagnostics;
-#ifdef OPENTRACK_API
- int iFpsArduino;
-#endif
- int CptError;
-
-
+ volatile int CptError;
};
-
-//*******************************************************************************************************
-// FaceTrackNoIR Tracker DLL. Functions used to get general info on the Tracker
-//*******************************************************************************************************
-#if defined(OPENTRACK_API)
class TrackerDll : public Metadata
{
QString name() { return QString("Hatire Arduino"); }
QIcon icon() { return QIcon(":/images/hat.png"); }
};
-#else
-class TrackerDll :
-public Metadata
-public ITrackerDll
-{
-public:
- TrackerDll();
- ~TrackerDll();
-
- void Initialize();
-
- QString name();
- QIcon icon();
- void getFullName(QString *strToBeFilled);
- void getShortName(QString *strToBeFilled);
- void getDescription(QString *strToBeFilled);
- void getIcon(QIcon *icon);
-
-private:
- QString trackerFullName; // Trackers' name and description
- QString trackerShortName;
- QString trackerDescription;
-};
-#endif
-
-#endif // FTNOIR_TRACKER_HAT_H
diff --git a/tracker-hatire/ftnoir_tracker_hat_dialog.cpp b/tracker-hatire/ftnoir_tracker_hat_dialog.cpp
index 3ef1a764..4ef83fb5 100644
--- a/tracker-hatire/ftnoir_tracker_hat_dialog.cpp
+++ b/tracker-hatire/ftnoir_tracker_hat_dialog.cpp
@@ -13,13 +13,6 @@
#include <QScrollBar>
-//*******************************************************************************************************
-// FaceTrackNoIR Client Settings-dialog.
-//*******************************************************************************************************
-
-//
-// Constructor for server-settings-dialog
-//
TrackerControls::TrackerControls() : theTracker(NULL), settingsDirty(false), timer(this)
{
@@ -180,7 +173,7 @@ TrackerControls::TrackerControls() : theTracker(NULL), settingsDirty(false), tim
connect(ui.QCB_Serial_flowControl, SIGNAL(currentIndexChanged(int)), this,SLOT(set_mod_flowControl(int)) );
connect(ui.btnReset, SIGNAL(clicked()), this, SLOT(doReset()));
- connect(ui.btnCenter, SIGNAL(clicked()), this, SLOT(doCenter()));
+ //connect(ui.btnCenter, SIGNAL(clicked()), this, SLOT(doCenter()));
connect(ui.btnZero, SIGNAL(clicked()), this, SLOT(doZero()));
connect(ui.btnSend, SIGNAL(clicked()), this, SLOT(doSend()));
@@ -209,8 +202,6 @@ void TrackerControls::Initialize(QWidget *parent) {
show();
}
-
-
//
// Apply online settings to tracker
//
@@ -220,27 +211,13 @@ void TrackerControls::settings_changed()
if (theTracker) theTracker->applysettings(settings);
}
-
-//
-// Center asked to ARDUINO
-//
-void TrackerControls::doCenter() {
-#ifdef OPENTRACK_API
- if (theTracker) theTracker->center();
-#else
- if (theTracker) theTracker->notifyCenter();
-#endif
-}
-
-
//
// Zero asked to ARDUINO
//
void TrackerControls::doZero() {
- if (theTracker) theTracker->notifyZeroed();
+ //if (theTracker) theTracker->notifyZeroed();
}
-
//
// Reset asked to ARDUINO
//
@@ -253,32 +230,28 @@ void TrackerControls::doReset() {
// Serial Info debug
//
void TrackerControls::doSerialInfo() {
- if (theTracker) theTracker->SerialInfo();
+ if (theTracker) theTracker->serial_info();
}
-
//
// Send command to ARDUINO
//
void TrackerControls::doSend() {
if (theTracker) {
if (!ui.lineSend->text().isEmpty()) {
- theTracker->sendcmd(ui.lineSend->text().toLatin1());
+ theTracker->send_serial_command(ui.lineSend->text().toLatin1());
}
}
}
-
//
// Enter on lineSend for send to ARDUINO
//
void TrackerControls::on_lineSend_returnPressed()
{
this->doSend();
-
}
-
//
// Display FPS of Arduino.
//
@@ -295,7 +268,6 @@ void TrackerControls::poll_tracker_info()
}
-
void TrackerControls::WriteMsgInfo(const QByteArray &MsgInfo)
{
QApplication::beep();
@@ -305,14 +277,11 @@ void TrackerControls::WriteMsgInfo(const QByteArray &MsgInfo)
bar->setValue(bar->maximum());
}
-
-
void TrackerControls::doSave() {
settingsDirty=false;
settings.save_ini();
}
-
//
// OK clicked on server-dialog
//
@@ -358,7 +327,7 @@ void TrackerControls::register_tracker(ITracker *tracker)
void TrackerControls::registerTracker(ITracker *tracker)
#endif
{
- theTracker = static_cast<FTNoIR_Tracker*>(tracker);
+ theTracker = static_cast<hatire*>(tracker);
connect(theTracker, SIGNAL(sendMsgInfo(QByteArray)),this , SLOT(WriteMsgInfo(QByteArray)));
if (isVisible() && settingsDirty) theTracker->applysettings(settings);
diff --git a/tracker-hatire/ftnoir_tracker_hat_dialog.h b/tracker-hatire/ftnoir_tracker_hat_dialog.h
index bd225b3e..7be31e37 100644
--- a/tracker-hatire/ftnoir_tracker_hat_dialog.h
+++ b/tracker-hatire/ftnoir_tracker_hat_dialog.h
@@ -1,11 +1,4 @@
-#ifndef FTNOIR_TRACKER_HAT_DIALOG_H
-#define FTNOIR_TRACKER_HAT_DIALOG_H
-
-#ifdef OPENTRACK_API
#include "opentrack/plugin-api.hpp"
-#else
-#include "..\ftnoir_tracker_base\ftnoir_tracker_base.h"
-#endif
#include "ftnoir_tracker_hat_settings.h"
#include "ftnoir_tracker_hat.h"
#include "ui_ftnoir_hatcontrols.h"
@@ -17,29 +10,19 @@
#include <QMetaType>
// Widget that has controls for FTNoIR protocol client-settings.
-#ifdef OPENTRACK_API
class TrackerControls: public ITrackerDialog
- #else
-class TrackerControls: public QWidget, public ITrackerDialog
- #endif
{
Q_OBJECT
public:
explicit TrackerControls();
~TrackerControls() override;
-#ifdef OPENTRACK_API
void Initialize(QWidget *parent) ; // unused
void register_tracker(ITracker *tracker) override;
void unregister_tracker() override;
-#else
- void Initialize(QWidget *parent) ;
- void registerTracker(ITracker *tracker) ;
- void unRegisterTracker() ;
-#endif
private:
Ui::UIHATControls ui;
- FTNoIR_Tracker *theTracker;
+ hatire *theTracker;
QTime last_time;
public slots:
@@ -83,9 +66,8 @@ protected slots:
void set_DelaySeq(int val) { settings.DelaySeq = val; settings_changed(); }
void set_endian(bool val) { settings.BigEndian = val; settings_changed(); }
-#ifdef OPENTRACK_API
+
void set_Fps(int val) { settings.FPSArduino = val; settings_changed(); }
-#endif
void set_mod_baud(int val) { settings.pBaudRate = static_cast<QSerialPort::BaudRate>(ui.QCB_Serial_baudRate->itemData(val).toInt()) ; settings_changed(); }
void set_mod_dataBits(int val) { settings.pDataBits = static_cast<QSerialPort::DataBits>(ui.QCB_Serial_dataBits->itemData(val).toInt()) ; settings_changed(); }
@@ -97,7 +79,7 @@ protected slots:
void doCancel();
void doSave();
void doReset();
- void doCenter();
+ //void doCenter();
void doZero();
void doSend();
void poll_tracker_info();
@@ -112,5 +94,3 @@ protected:
private slots:
void on_lineSend_returnPressed();
};
-
-#endif //FTNOIR_TRACKER_HAT_DIALOG_H
diff --git a/tracker-hatire/ftnoir_tracker_hat_settings.cpp b/tracker-hatire/ftnoir_tracker_hat_settings.cpp
index df0480a1..02c99e75 100644
--- a/tracker-hatire/ftnoir_tracker_hat_settings.cpp
+++ b/tracker-hatire/ftnoir_tracker_hat_settings.cpp
@@ -13,18 +13,13 @@
#include <QVariant>
#include "ftnoir_tracker_hat_settings.h"
-#ifdef OPENTRACK_API
#include "opentrack-compat/options.hpp"
-#endif
+
+// XXX TODO move to opentrack settings api -sh 20160410
void TrackerSettings::load_ini()
{
-#ifndef OPENTRACK_API
- QSettings settings(OPENTRACK_ORG); // Registry settings (in HK_USER)
- QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
-#else
QString currentFile = options::group::ini_pathname();
-#endif
QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file)
iniFile.beginGroup( "HAT" );
@@ -66,9 +61,8 @@ void TrackerSettings::load_ini()
DelayStart=iniFile.value("DelayStart",0).toInt();
DelaySeq=iniFile.value("DelaySeq",0).toInt();
-#ifdef OPENTRACK_API
FPSArduino=iniFile.value("FPSArduino",30).toInt();
-#endif
+
BigEndian=iniFile.value("BigEndian",0).toBool();
@@ -84,12 +78,8 @@ void TrackerSettings::load_ini()
void TrackerSettings::save_ini() const
{
-#ifndef OPENTRACK_API
- QSettings settings(OPENTRACK_ORG); // Registry settings (in HK_USER)
- QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
-#else
QString currentFile = options::group::ini_pathname();
-#endif
+
QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file)
iniFile.beginGroup ( "HAT" );
@@ -129,9 +119,8 @@ void TrackerSettings::save_ini() const
iniFile.setValue ( "DelayStart",DelayStart);
iniFile.setValue ( "DelaySeq",DelaySeq);
-#ifdef OPENTRACK_API
iniFile.setValue ( "FPSArduino", FPSArduino );
-#endif
+
iniFile.setValue("BigEndian",BigEndian);
iniFile.setValue("BaudRate",pBaudRate);
@@ -143,4 +132,3 @@ void TrackerSettings::save_ini() const
iniFile.endGroup();
}
-
diff --git a/tracker-hatire/ftnoir_tracker_hat_settings.h b/tracker-hatire/ftnoir_tracker_hat_settings.h
index ade46d54..bed33200 100644
--- a/tracker-hatire/ftnoir_tracker_hat_settings.h
+++ b/tracker-hatire/ftnoir_tracker_hat_settings.h
@@ -5,12 +5,10 @@
* copyright notice and this permission notice appear in all copies.
*/
-#ifndef FTNOIR_TRACKER_HAT_SETTINGS_H
-#define FTNOIR_TRACKER_HAT_SETTINGS_H
+#pragma once
#include <QtSerialPort/QSerialPort>
-//-----------------------------------------------------------------------------
struct TrackerSettings
{
void load_ini();
@@ -30,7 +28,6 @@ struct TrackerSettings
bool InvertY;
bool InvertZ;
-
int RollAxe;
int PitchAxe;
int YawAxe;
@@ -59,10 +56,5 @@ struct TrackerSettings
QSerialPort::StopBits pStopBits;
QSerialPort::FlowControl pFlowControl;
-#ifdef OPENTRACK_API
int FPSArduino;
-#endif
};
-
-
-#endif //FTNOIR_TRACKER_HAT_SETTINGS_H
diff --git a/tracker-hatire/thread.cpp b/tracker-hatire/thread.cpp
new file mode 100644
index 00000000..26bdc14d
--- /dev/null
+++ b/tracker-hatire/thread.cpp
@@ -0,0 +1,300 @@
+#include "thread.hpp"
+#include "opentrack-compat/sleep.hpp"
+#include <utility>
+
+#include <QTextStream>
+#include <QTime>
+#include <QDebug>
+
+#include <cstring>
+
+void hatire_thread::Log(const QString& message)
+{
+ // Drop out immediately if logging is off. Yes, there is still some overhead because of passing strings around for no reason.
+ // that's unfortunate and I'll monitor the impact and see if it needs a more involved fix.
+ if (!s.bEnableLogging) return;
+
+ Diag flDiagnostics;
+
+ if (flDiagnostics.open(QIODevice::ReadWrite | QIODevice::Append))
+ {
+ QTextStream out(&flDiagnostics);
+ QString milliSeconds;
+ milliSeconds = QString("%1").arg(QTime::currentTime().msec(), 3, 10, QChar('0'));
+ // We have a file
+ out << QTime::currentTime().toString() << "." << milliSeconds << ": " << message << "\r\n";
+ flDiagnostics.close();
+ }
+}
+
+void hatire_thread::start(const thread_settings& s_)
+{
+ s = s_;
+ com_port.moveToThread(this);
+#ifdef HATIRE_DEBUG_LOGFILE
+ read_timer.moveToThread(this);
+#endif
+ QThread::start();
+}
+
+hatire_thread::~hatire_thread()
+{
+ quit();
+ wait();
+}
+
+thread_settings hatire_thread::serial_settings_impl()
+{
+ return s;
+}
+
+hatire_thread::hatire_thread()
+{
+ data_read.reserve(65536);
+
+ connect(this, &QThread::finished, this, &hatire_thread::teardown_serial);
+ connect(this, &hatire_thread::update_serial_settings, this, &hatire_thread::update_serial_settings_impl, Qt::QueuedConnection);
+ 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);
+ connect(this, &hatire_thread::sendcmd, this, &hatire_thread::sendcmd_impl, Qt::QueuedConnection);
+ connect(this, &hatire_thread::serial_settings, this, &hatire_thread::serial_settings_impl, Qt::QueuedConnection);
+}
+
+void hatire_thread::teardown_serial()
+{
+ if (isRunning() && com_port.isOpen())
+ {
+ QByteArray msg;
+ Log("Tracker shut down");
+ com_port.write(s.sCmdStop);
+ if (!com_port.waitForBytesWritten(1000))
+ {
+ emit serial_debug_info("TimeOut in writing CMD");
+ }
+ else
+ {
+ msg.append("\r\n");
+ msg.append("SEND '");
+ msg.append(s.sCmdStop);
+ msg.append("'\r\n");
+ }
+ emit serial_debug_info(msg);
+
+ disconnect(&com_port, SIGNAL(readyRead()), nullptr, nullptr);
+ com_port.close();
+ }
+}
+
+void hatire_thread::run()
+{
+#ifdef HATIRE_DEBUG_LOGFILE
+ com_port.setFileName(HATIRE_DEBUG_LOGFILE);
+ com_port.open(QIODevice::ReadOnly);
+
+ connect(&read_timer, &QTimer::timeout, this, &hatire_thread::on_serial_read, Qt::DirectConnection);
+ read_timer.start(16);
+#else
+ connect(&com_port, &serial_t::readyRead, this, &hatire_thread::on_serial_read, Qt::DirectConnection);
+#endif
+ (void) exec();
+}
+
+void hatire_thread::update_serial_settings_impl(const thread_settings &s_)
+{
+ s = s_;
+}
+
+serial_result hatire_thread::init_serial_port_impl()
+{
+#ifndef HATIRE_DEBUG_LOGFILE
+ com_port.setPortName(s.sSerialPortName);
+
+ if (com_port.open(QIODevice::ReadWrite))
+ {
+ Log("Port Open");
+ if (
+ com_port.setBaudRate((QSerialPort::BaudRate)s.iBaudRate)
+ && com_port.setDataBits((QSerialPort::DataBits)s.iDataBits)
+ && com_port.setParity((QSerialPort::Parity)s.iParity)
+ && com_port.setStopBits((QSerialPort::StopBits)s.iStopBits)
+ && com_port.setFlowControl((QSerialPort::FlowControl)s.iFlowControl)
+ && com_port.clear(QSerialPort::AllDirections)
+ && com_port.setDataErrorPolicy(QSerialPort::IgnorePolicy)
+ )
+ {
+ Log("Port Parameters set");
+ qDebug() << QTime::currentTime() << " HAT OPEN on " << com_port.portName() << com_port.baudRate() << com_port.dataBits() << com_port.parity() << com_port.stopBits() << com_port.flowControl();
+
+ if (com_port.flowControl() == QSerialPort::HardwareControl)
+ {
+ // Raise DTR
+ Log("Raising DTR");
+ if (!com_port.setDataTerminalReady(true))
+ Log("Couldn't set DTR");
+
+ // Raise RTS/CTS
+ Log("Raising RTS");
+ if (!com_port.setRequestToSend(true))
+ Log("Couldn't set RTS");
+ }
+ // Wait init arduino sequence
+ for (int i = 1; i <=s.iDelayInit; i+=50) {
+ if (com_port.waitForReadyRead(50)) break;
+ }
+ Log("Waiting on init");
+ qDebug() << QTime::currentTime() << " HAT send INIT ";
+ sendcmd(s.sCmdInit);
+ // Wait init MPU sequence
+ for (int i = 1; i <=s.iDelayStart; i+=50) {
+ if (com_port.waitForReadyRead(50)) break;
+ }
+ // Send START cmd to IMU
+ qDebug() << QTime::currentTime() << " HAT send START ";
+ sendcmd(s.sCmdStart);
+
+ // Wait start MPU sequence
+ for (int i = 1; i <=s.iDelaySeq; i+=50) {
+ if (com_port.waitForReadyRead(50)) break;
+ }
+ Log("Port setup, waiting for HAT frames to process");
+ qDebug() << QTime::currentTime() << " HAT wait MPU ";
+
+ return serial_result();
+ }
+ else
+ {
+ return serial_result(result_error, com_port.errorString());
+ }
+ }
+ else
+ return serial_result(result_open_error, com_port.errorString());
+#else
+ return serial_result();
+#endif
+}
+
+// Info SerialPort
+void hatire_thread::serial_info_impl()
+{
+#ifndef HATIRE_DEBUG_LOGFILE
+ QByteArray msg;
+
+ if (com_port.isOpen())
+ {
+ msg.append("\r\n");
+ msg.append(com_port.portName());
+ msg.append("\r\n");
+ msg.append("BAUDRATE :");
+ msg.append(QString::number(com_port.baudRate()));
+ msg.append("\r\n");
+ msg.append("DataBits :");
+ msg.append(QString::number(com_port.dataBits()));
+ msg.append("\r\n");
+ msg.append("Parity :");
+
+ switch (com_port.parity())
+ {
+ case 0: msg.append("No parity");
+ break;
+ case 2: msg.append("Even parity");
+ break;
+ case 3: msg.append("Odd parity");
+ break;
+ case 4: msg.append("Space parity");
+ break;
+ case 5: msg.append("Mark parity");
+ break;
+ default: msg.append("Unknown parity");
+ break;
+ }
+
+ msg.append("\r\n");
+ msg.append("Stop Bits :");
+
+ switch (com_port.stopBits())
+ {
+ msg.append(QString::number(com_port.stopBits()));
+ case 1: msg.append("1 stop bit.");
+ break;
+ case 2: msg.append("2 stop bits.");
+ break;
+ case 3: msg.append("1.5 stop bits.");
+ break;
+ default: msg.append("Unknown number of stop bit.");
+ break;
+ }
+
+ msg.append("\r\n");
+ msg.append("Flow Control :");
+ switch (com_port.flowControl())
+ {
+ case 0: msg.append("No flow control");
+ break;
+ case 1: msg.append("Hardware flow control (RTS/CTS)");
+ break;
+ case 2: msg.append("Software flow control (XON/XOFF)");
+ break;
+ default: msg.append("Unknown flow control");
+ break;
+ }
+
+ emit serial_debug_info(msg);
+ }
+#endif
+}
+
+#ifdef __GNUC__
+# define unused(t, i) t __attribute__((unused)) i
+#else
+# define unused(t, i) t i
+#endif
+
+//send command to Arduino
+
+
+void hatire_thread::on_serial_read()
+{
+ static constexpr int ms = 1000/60;
+
+ {
+ 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)
+ {
+ qDebug() << "eof";
+ read_timer.stop();
+ }
+#endif
+ }
+ // qt can fire QSerialPort::readyRead() needlessly, causing a busy loop.
+ // see https://github.com/opentrack/opentrack/issues/327#issuecomment-207941003
+ portable::sleep(ms);
+}
+
+void hatire_thread::prepend_unread_data(const QByteArray &data)
+{
+ QMutexLocker lck(&data_mtx);
+ data_read.prepend(data);
+}
+
+QByteArray hatire_thread::flush_data_read()
+{
+ QMutexLocker lck(&data_mtx);
+
+ constexpr int packet_len = 30;
+
+ if (data_read.length() < 4 * packet_len)
+ {
+ // we're requesting more than packet length to help resync the stream if needed
+ return QByteArray();
+ }
+
+ QByteArray ret = data_read.left(90);
+ data_read = data_read.mid(90);
+
+ return ret;
+}
diff --git a/tracker-hatire/thread.hpp b/tracker-hatire/thread.hpp
new file mode 100644
index 00000000..dd0d8c14
--- /dev/null
+++ b/tracker-hatire/thread.hpp
@@ -0,0 +1,167 @@
+#pragma once
+
+#include <QSerialPort>
+#include <QByteArray>
+#include <QThread>
+#include <QMutex>
+
+#include <QFile>
+#include <QCoreApplication>
+
+enum results
+{
+ result_ok,
+ result_open_error,
+ result_error,
+};
+
+//#define HATIRE_DEBUG_LOGFILE "d:/putty-hatire.log"
+
+#ifdef HATIRE_DEBUG_LOGFILE
+# include <QFile>
+# include <QTimer>
+#endif
+
+struct thread_settings
+{
+ QByteArray sCmdStart;
+ QByteArray sCmdStop;
+ QByteArray sCmdInit;
+ QByteArray sCmdReset;
+ QByteArray sCmdCenter;
+ QByteArray sCmdZero;
+
+ QString sSerialPortName;
+ QSerialPort::BaudRate iBaudRate;
+ QSerialPort::DataBits iDataBits;
+ QSerialPort::Parity iParity;
+ QSerialPort::StopBits iStopBits;
+ QSerialPort::FlowControl iFlowControl;
+
+ int iDelayInit;
+ int iDelayStart;
+ int iDelaySeq;
+ bool bBigEndian;
+ volatile bool bEnableLogging;
+
+ thread_settings() :
+ iBaudRate(QSerialPort::UnknownBaud),
+ iDataBits(QSerialPort::UnknownDataBits),
+ iParity(QSerialPort::UnknownParity),
+ iStopBits(QSerialPort::UnknownStopBits),
+ iFlowControl(QSerialPort::UnknownFlowControl),
+ iDelayInit(0),
+ iDelayStart(0),
+ iDelaySeq(0),
+ bBigEndian(false),
+ bEnableLogging(false)
+ {
+ }
+};
+
+#include <QMetaType>
+
+Q_DECLARE_METATYPE(thread_settings)
+
+struct serial_result
+{
+ serial_result() : code(result_ok) {}
+ serial_result(results code, const QString& error) : error(error), code(code) {}
+
+ QString error;
+ results code;
+};
+
+struct Diag : public QFile
+{
+ Diag()
+ {
+ setFileName(QCoreApplication::applicationDirPath() + "/HATDiagnostics.txt");
+ }
+};
+
+class hatire_thread : public QThread
+{
+ Q_OBJECT
+
+#ifdef HATIRE_DEBUG_LOGFILE
+ using serial_t = QFile;
+ QTimer read_timer;
+#else
+ using serial_t = QSerialPort;
+#endif
+
+ QByteArray data_read;
+ serial_t com_port;
+ thread_settings s;
+ QMutex data_mtx;
+
+ void run() override;
+
+private slots:
+ void on_serial_read();
+ void teardown_serial();
+
+ void sendcmd_impl(const QByteArray& cmd)
+ {
+#ifndef HATIRE_DEBUG_LOGFILE
+ QByteArray Msg;
+
+ if (cmd.length() > 0)
+ {
+ if (com_port.isOpen())
+ {
+ QString logMess;
+ logMess.append("SEND '");
+ logMess.append(cmd);
+ logMess.append("'");
+ Log(logMess);
+ com_port.write(cmd);
+ if (!com_port.waitForBytesWritten(1000)) {
+ emit serial_debug_info("TimeOut in writing CMD");
+ } else
+ {
+ Msg.append("\r\n");
+ Msg.append("SEND '");
+ Msg.append(cmd);
+ Msg.append("'\r\n");
+ }
+#if 0 // WaitForReadyRead isn't working well and there are some reports of it being a win32 issue. We can live without it anyway
+ if ( !com_port.waitForReadyRead(1000)) {
+ emit serial_debug_info("TimeOut in response to CMD") ;
+ } else {
+ emit serial_debug_info(Msg);
+ }
+#else
+ emit serial_debug_info(Msg);
+#endif
+ } else {
+ emit serial_debug_info("ComPort not open") ;
+ }
+ }
+#endif
+ }
+ void serial_info_impl();
+ serial_result init_serial_port_impl();
+ void update_serial_settings_impl(const thread_settings& s);
+ thread_settings serial_settings_impl();
+
+signals:
+ void serial_debug_info(const QByteArray &MsgInfo);
+
+ void sendcmd(const QByteArray& cmd);
+ void serial_info();
+ serial_result init_serial_port();
+ void update_serial_settings(const thread_settings& s);
+ thread_settings serial_settings();
+
+public:
+ void start(const thread_settings &s_);
+ ~hatire_thread() override;
+ hatire_thread();
+
+ void prepend_unread_data(const QByteArray& data);
+
+ QByteArray flush_data_read();
+ void Log(const QString& message);
+};