path: root/tracker-hatire/ftnoir_tracker_hat.cpp

/* 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