move to subdirectory-based build system

Closes #224

1 files changed, 597 insertions, 0 deletions

diff --git a/tracker-hatire/ftnoir_tracker_hat.cpp b/tracker-hatire/ftnoir_tracker_hat.cpp
new file mode 100644
index 00000000..eea1cbca
--- /dev/null
+++ b/tracker-hatire/ftnoir_tracker_hat.cpp
@@ -0,0 +1,597 @@
+/* Homepage         http://facetracknoir.sourceforge.net/home/default.htm        *
+ *                                                                               *
+ * ISC License (ISC)                                                             *
+ *                                                                               *
+ * Copyright (c) 2015, Wim Vriend                                                *
+ *                                                                               *
+ * Permission to use, copy, modify, and/or distribute this software for any      *
+ * purpose with or without fee is hereby granted, provided that the above        *
+ * copyright notice and this permission notice appear in all copies.             *
+ */
+#include <QDebug>
+#include "ftnoir_tracker_hat.h"
+
+FTNoIR_Tracker::FTNoIR_Tracker()
+{
+    qDebug()<<"Tracker::HAT";
+
+	ComPort =   NULL;
+
+	HAT.Rot[0]=0;
+	HAT.Rot[1]=0;
+	HAT.Rot[2]=0;
+	HAT.Trans[0]=0;
+	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()
+{
+    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")  ;
+		}
+	}
+}
+
+
+// return FPS 
+void FTNoIR_Tracker::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 )
+{
+	CptError=0;
+	dataRead.clear();
+	frame_cnt=0;
+	
+	Log("INITIALISING HATIRE");
+
+	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;
+}
+
+
+
+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;
+}
+
+
+void FTNoIR_Tracker::StopTracker( bool exit )
+{
+	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);
+}
+
+
+#else
+void FTNoIR_Tracker::start_tracker(QFrame*)
+{
+	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;
+
+}
+
+//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
+{
+    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 );
+			datastream>>ArduinoData;
+			frame_cnt++;
+			if (ArduinoData.Code <= 1000) {
+				HAT=ArduinoData;
+                new_frame=true;
+			} else {
+				emit sendMsgInfo(dataRead.mid(4,24))  ;
+			}
+			dataRead.remove(0,30);
+		} else {
+			// resynchro trame 
+			int index =	dataRead.indexOf(Begin);
+			if (index==-1) {
+				index=dataRead.length();
+			} 
+			emit sendMsgInfo(dataRead.mid(0,index))  ;
+			dataRead.remove(0,index);
+			CptError++;
+			qDebug() << QTime::currentTime() << " HAT Resync-Frame, counter " << CptError;
+		}
+	}
+
+	if (CptError>50) {
+		emit sendMsgInfo("Can't find HAT frame")  ;
+		CptError=0;
+#ifndef OPENTRACK_API
+		return false;
+#endif
+	}
+	// 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];
+			
+    } else data[Yaw] =0;
+
+	if (bEnablePitch) {
+        if (bInvertPitch) data[Pitch] =  HAT.Rot[iPitchAxe] *  -1.0f;
+        else data[Pitch] =   HAT.Rot[iPitchAxe];
+    } else data[Pitch] = 0;
+
+	if (bEnableRoll) {
+        if (bInvertRoll) data[Roll] =  HAT.Rot[iRollAxe] *  -1.0f;
+        else data[Roll] =  HAT.Rot[iRollAxe];
+    } else data[Roll] =0;
+
+	if (bEnableX) {
+        if (bInvertX) data[TX] =  HAT.Trans[iXAxe]*  -1.0f;
+        else data[TX] =   HAT.Trans[iXAxe];
+    } else data[TX] =0;
+
+	if (bEnableY) {
+        if (bInvertY) data[TY] =  HAT.Trans[iYAxe]*  -1.0f;
+        else data[TY] =  HAT.Trans[iYAxe];
+    } else data[TY] =0;
+
+	if (bEnableZ) {
+        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];
+	}
+
+	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;
+
+	bEnableRoll = settings.EnableRoll;
+	bEnablePitch = settings.EnablePitch;
+	bEnableYaw = settings.EnableYaw;
+	bEnableX = settings.EnableX;
+	bEnableY = settings.EnableY;
+	bEnableZ = settings.EnableZ;
+
+	bInvertRoll = settings.InvertRoll;
+	bInvertPitch = settings.InvertPitch;
+	bInvertYaw = settings.InvertYaw;
+	bInvertX = settings.InvertX;
+	bInvertY = settings.InvertY;
+	bInvertZ = settings.InvertZ;
+	bEnableLogging = settings.EnableLogging;
+
+	iRollAxe= settings.RollAxe;
+	iPitchAxe= settings.PitchAxe;
+	iYawAxe= settings.YawAxe;
+	iXAxe= settings.XAxe;
+	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();
+	}
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Factory function that creates instances if the Tracker object.
+
+// 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
+