From 4da4b7608ff03782ed2a157a7ef2bc843101b271 Mon Sep 17 00:00:00 2001 From: martin Date: Sat, 2 May 2015 08:42:50 +1000 Subject: Fix returning consistent data to get headpose and other minor issues --- ftnoir_tracker_hatire/ftnoir_hatcontrols.ui | 16 ++++- ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp | 83 +++++++++++++++++++--- ftnoir_tracker_hatire/ftnoir_tracker_hat.h | 6 +- .../ftnoir_tracker_hat_dialog.cpp | 3 + ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.h | 2 + .../ftnoir_tracker_hat_settings.cpp | 5 +- .../ftnoir_tracker_hat_settings.h | 1 + 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; } - + @@ -1129,14 +1129,14 @@ p, li { white-space: pre-wrap; } - + Send - + Qt::ActionsContextMenu @@ -1158,6 +1158,16 @@ p, li { white-space: pre-wrap; } + + + + Disable when not in use, will have a performance impact + + + Enable logging to diagnostic file + + + 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 #include -#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; -- cgit v1.2.3