summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_hatire
diff options
context:
space:
mode:
authormartin <martin@thegreens.net>2015-05-02 08:42:50 +1000
committermartin <martin@thegreens.net>2015-05-02 08:42:50 +1000
commit4da4b7608ff03782ed2a157a7ef2bc843101b271 (patch)
tree3189b57a6a3cd901c44387a877f35ecacbf296cc /ftnoir_tracker_hatire
parent0dbfd34bab63170915b9bdd0f456f39def4ea43e (diff)
Fix returning consistent data to get headpose and other minor issues
Diffstat (limited to 'ftnoir_tracker_hatire')
-rw-r--r--ftnoir_tracker_hatire/ftnoir_hatcontrols.ui16
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp83
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat.h6
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.cpp3
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat_dialog.h2
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.cpp5
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat_settings.h1
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;