Fix returning consistent data to get headpose and other minor issues

7 files changed, 102 insertions, 14 deletions

diff --git a/ftnoir_tracker_hatire/ftnoir_hatcontrols.ui b/ftnoir_tracker_hatire/ftnoir_hatcontrols.ui
index 1611489b..2d347f08 100644
--- a/ftnoir_tracker_hatire/ftnoir_hatcontrols.ui
+++ b/ftnoir_tracker_hatire/ftnoir_hatcontrols.ui
@@ -1119,7 +1119,7 @@ p, li { white-space: pre-wrap; }
          </property>
         </widget>
        </item>
-       <item row="2" column="0" colspan="2">
+       <item row="3" column="0" colspan="2">
         <widget class="QLineEdit" name="lineSend">
          <property name="inputMask">
           <string notr="true"/>
@@ -1129,14 +1129,14 @@ p, li { white-space: pre-wrap; }
          </property>
         </widget>
        </item>
-       <item row="2" column="2">
+       <item row="3" column="2">
         <widget class="QPushButton" name="btnSend">
          <property name="text">
           <string>Send</string>
          </property>
         </widget>
        </item>
-       <item row="3" column="0" colspan="3">
+       <item row="4" column="0" colspan="3">
         <widget class="QPlainTextEdit" name="pteINFO">
          <property name="contextMenuPolicy">
           <enum>Qt::ActionsContextMenu</enum>
@@ -1158,6 +1158,16 @@ p, li { white-space: pre-wrap; }
          </property>
         </widget>
        </item>
+       <item row="2" column="0" colspan="3">
+        <widget class="QCheckBox" name="chkEnableLogging">
+         <property name="toolTip">
+          <string>Disable when not in use, will have a performance impact</string>
+         </property>
+         <property name="text">
+          <string>Enable logging to diagnostic file</string>
+         </property>
+        </widget>
+       </item>
       </layout>
      </widget>
     </widget>
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp b/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp
index 9f73067d..c3ec6029 100644
--- a/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp
@@ -50,6 +50,8 @@ FTNoIR_Tracker::FTNoIR_Tracker()
 	Begin.append((char) 0xAA);
 	End.append((char) 0x55);
 	End.append((char) 0x55);
+	
+	flDiagnostics.setFileName(QCoreApplication::applicationDirPath() + "/HATDiagnostics.txt");
 
 	settings.load_ini();
 }
@@ -62,10 +64,12 @@ FTNoIR_Tracker::~FTNoIR_Tracker()
 
 #ifdef OPENTRACK_API
             QByteArray Msg;
+			Log("Tracker shut down");
             ComPort->write(sCmdStop);
             if (!ComPort->waitForBytesWritten(1000)) {
                 emit sendMsgInfo("TimeOut in writing CMD");
-            } else  {
+            } else  
+			{
                 Msg.append("\r\n");
                 Msg.append("SEND '");
                 Msg.append(sCmdStop);
@@ -166,20 +170,30 @@ void FTNoIR_Tracker::sendcmd(const QByteArray &cmd) {
 	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  {
+			} else  
+			{
 				Msg.append("\r\n");
 				Msg.append("SEND '");
 				Msg.append(cmd);
 				Msg.append("'\r\n");
 			}
+			#ifndef _WIN32 // 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")  ;
 		}
@@ -195,7 +209,7 @@ void FTNoIR_Tracker::get_info( int *tps ){
 
 void FTNoIR_Tracker::SerialRead()
 {
-    QMutexLocker lck(&mutex);
+	QMutexLocker lck(&mutex);
 	dataRead+=ComPort->readAll();
 }
 
@@ -205,6 +219,8 @@ void FTNoIR_Tracker::Initialize( QFrame *videoframe )
 	CptError=0;
 	dataRead.clear();
 	frame_cnt=0;
+	
+	Log("INITIALISING HATIRE");
 
 	settings.load_ini();
 	applysettings(settings);
@@ -249,6 +265,7 @@ 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;
@@ -260,13 +277,16 @@ void FTNoIR_Tracker::StartTracker(HWND parent_window)
 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  {
+			} else  
+			{
 				Msg.append("\r\n");
 				Msg.append("SEND '");
 				Msg.append(sCmdStop);
@@ -297,8 +317,11 @@ void FTNoIR_Tracker::start_tracker(QFrame*)
 	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) 
@@ -308,12 +331,27 @@ void FTNoIR_Tracker::start_tracker(QFrame*)
 			&& 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 
@@ -328,6 +366,7 @@ void FTNoIR_Tracker::start_tracker(QFrame*)
 				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,"FaceTrackNoIR Error", ComPort->errorString(),QMessageBox::Ok,QMessageBox::NoButton);
@@ -345,6 +384,8 @@ void FTNoIR_Tracker::start_tracker(QFrame*)
 //send CENTER to Arduino
 void FTNoIR_Tracker::center() {
     qDebug()   << " HAT send CENTER ";
+	Log("Sending Centre Command");
+
     sendcmd(sCmdCenter);
 }
 
@@ -368,6 +409,7 @@ bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *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 );
@@ -390,6 +432,7 @@ bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data)
 			emit sendMsgInfo(dataRead.mid(0,index))  ;
 			dataRead.remove(0,index);
 			CptError++;
+			qDebug() << QTime::currentTime() << " HAT Resync-Frame, counter " << CptError;
 		}
 	}
 
@@ -400,13 +443,19 @@ bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data)
 		return false;
 #endif
 	}
-    if  (new_frame) {
+	// 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;
+			
+    } else data[Yaw] =0;
 
 	if (bEnablePitch) {
         if (bInvertPitch) data[Pitch] =  HAT.Rot[iPitchAxe] *  -1.0f;
@@ -433,7 +482,7 @@ bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data)
         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];
@@ -494,6 +543,7 @@ void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){
 	bInvertX = settings.InvertX;
 	bInvertY = settings.InvertY;
 	bInvertZ = settings.InvertZ;
+	bEnableLogging = settings.EnableLogging;
 
 	iRollAxe= settings.RollAxe;
 	iPitchAxe= settings.PitchAxe;
@@ -525,6 +575,23 @@ void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){
 #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();
+	}
+}
 
 
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat.h b/ftnoir_tracker_hatire/ftnoir_tracker_hat.h
index 2abeb77d..020b4132 100644
--- a/ftnoir_tracker_hatire/ftnoir_tracker_hat.h
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat.h
@@ -19,7 +19,7 @@
 #include <QMutexLocker>
 #include <QSettings>
 
-#define VER_FILEVERSION_STR         "Version 2.1.0\0"
+#define VER_FILEVERSION_STR         "Version 2.1.1\0"
 
 class FTNoIR_Tracker : public QObject, public ITracker 
 { 
@@ -49,6 +49,7 @@ public:
 
 private Q_SLOTS:
     void SerialRead();
+	void Log(QString message);
 
 signals:
     void sendMsgInfo(const QByteArray &MsgInfo);
@@ -80,6 +81,7 @@ private:
 	bool bInvertX;
 	bool bInvertY;
 	bool bInvertZ;
+	bool bEnableLogging;
 
 	int iRollAxe;
 	int iPitchAxe;
@@ -107,6 +109,8 @@ private:
 	QSerialPort::Parity iParity;
 	QSerialPort::StopBits iStopBits;
 	QSerialPort::FlowControl iFlowControl;
+	
+	QFile flDiagnostics;
 #ifdef OPENTRACK_API
     int iFpsArduino;
 #endif
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.cpp b/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.cpp
index 51d5918e..b22e3b15 100644
--- a/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.cpp
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.cpp
@@ -117,6 +117,8 @@ TrackerControls::TrackerControls() : theTracker(NULL), settingsDirty(false), tim
     ui.chkInvertX->setChecked(settings.InvertX);
     ui.chkInvertY->setChecked(settings.InvertY);
     ui.chkInvertZ->setChecked(settings.InvertZ);
+	
+	ui.chkEnableLogging->setChecked(settings.EnableLogging);
 
 
     ui.cb_roll->setCurrentIndex(settings.RollAxe);
@@ -165,6 +167,7 @@ TrackerControls::TrackerControls() : theTracker(NULL), settingsDirty(false), tim
     connect( ui.chkInvertX,SIGNAL(toggled(bool)),			 this,SLOT(set_inv_x(bool)) );
     connect( ui.chkInvertY,SIGNAL(toggled(bool)),			 this,SLOT(set_inv_y(bool)) );
     connect( ui.chkInvertZ,SIGNAL(toggled(bool)),			 this,SLOT(set_inv_z(bool)) );
+	connect( ui.chkEnableLogging,SIGNAL(toggled(bool)),      this,SLOT(set_diag_logging(bool)) );
 
     connect(ui.cb_roll, SIGNAL(currentIndexChanged(int)), this,SLOT(set_rot_roll(int)));
     connect(ui.cb_pitch, SIGNAL(currentIndexChanged(int)),this,SLOT(set_rot_pitch(int)));
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.h b/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.h
index cb5e84b5..5d3e2c36 100644
--- a/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.h
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.h
@@ -61,6 +61,8 @@ protected slots:
 	void set_inv_y(bool val)	 { settings.InvertY = val;       settings_changed(); }
 	void set_inv_z(bool val)	 { settings.InvertZ = val;       settings_changed(); }
 
+	void set_diag_logging(bool val)	 { settings.EnableLogging = val;       settings_changed(); }
+
 
 	void set_rot_roll(int val)	 { settings.RollAxe = val;    settings_changed(); }
 	void set_rot_pitch(int val)	 { settings.PitchAxe = val;   settings_changed(); }
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.cpp b/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.cpp
index a327c0c1..017a8f0a 100644
--- a/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.cpp
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.cpp
@@ -57,8 +57,8 @@ void TrackerSettings::load_ini()
 	EnableX = iniFile.value( "EnableX", 0 ).toBool();
 	EnableY = iniFile.value( "EnableY", 0 ).toBool();
 	EnableZ = iniFile.value( "EnableZ", 0 ).toBool();
-
-
+	EnableLogging = iniFile.value( "EnableLogging", 0).toBool();
+	
 	InvertRoll = iniFile.value( "InvertRoll", 1 ).toBool();
 	InvertPitch = iniFile.value( "InvertPitch", 1 ).toBool();
 	InvertYaw = iniFile.value( "InvertYaw", 1 ).toBool();
@@ -127,6 +127,7 @@ void TrackerSettings::save_ini() const
 	iniFile.setValue( "EnableX", EnableX );
 	iniFile.setValue( "EnableY", EnableY );
 	iniFile.setValue( "EnableZ", EnableZ );
+	iniFile.setValue( "EnableLogging", EnableLogging );
 
     iniFile.setValue( "InvertRoll", InvertRoll );
 	iniFile.setValue( "InvertPitch", InvertPitch );
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.h b/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.h
index 5200e63f..5754da8c 100644
--- a/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.h
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.h
@@ -51,6 +51,7 @@ struct TrackerSettings
 	int DelaySeq;
 
 	bool BigEndian;
+	bool EnableLogging;
 
 	QString SerialPortName;
 	QSerialPort::BaudRate pBaudRate;