summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp')
-rw-r--r--ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp304
1 files changed, 304 insertions, 0 deletions
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp b/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp
new file mode 100644
index 00000000..e961a10f
--- /dev/null
+++ b/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp
@@ -0,0 +1,304 @@
+/********************************************************************************
+* FaceTrackNoIR This program is a private project of some enthusiastic *
+* gamers from Holland, who don't like to pay much for *
+* head-tracking. *
+* *
+* Copyright (C) 2012 Wim Vriend (Developing) *
+* Ron Hendriks (Researching and Testing) *
+* *
+* Homepage: http://facetracknoir.sourceforge.net/home/default.htm *
+* *
+* This program is free software; you can redistribute it and/or modify it *
+* under the terms of the GNU General Public License as published by the *
+* Free Software Foundation; either version 3 of the License, or (at your *
+* option) any later version. *
+* *
+* This program is distributed in the hope that it will be useful, but *
+* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
+* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for *
+* more details. *
+* *
+* You should have received a copy of the GNU General Public License along *
+* with this program; if not, see <http://www.gnu.org/licenses/>. *
+* *
+********************************************************************************/
+
+
+#include "ftnoir_tracker_hat.h"
+#include <QMessageBox>
+#include <QDebug>
+#include <QCoreApplication>
+
+FTNoIR_Tracker::FTNoIR_Tracker()
+{
+ SerialPort = NULL;
+ waitTimeout = 1000;
+ TrackerSettings settings;
+ settings.load_ini();
+ applysettings(settings);
+
+
+ // Create events
+ m_StopThread = CreateEvent(0, TRUE, FALSE, 0);
+ m_WaitThread = CreateEvent(0, TRUE, FALSE, 0);
+
+ ListErrInf = new QList<QString>();
+
+ // prepare & reserve QByteArray
+ datagram.reserve(30);
+}
+
+FTNoIR_Tracker::~FTNoIR_Tracker()
+{
+ // Trigger thread to stop
+ ::SetEvent(m_StopThread);
+
+ // Wait until thread finished
+ if (isRunning()) {
+ ::WaitForSingleObject(m_WaitThread, INFINITE);
+ }
+
+ // Close handles
+ ::CloseHandle(m_StopThread);
+ ::CloseHandle(m_WaitThread);
+
+ if (SerialPort!=NULL) {
+ if (SerialPort->isOpen() ) {
+ SerialPort->putChar('s'); //Send STOP to Arduino
+ SerialPort->close();
+ }
+ delete SerialPort;
+ SerialPort=NULL;
+ }
+
+}
+
+//send CENTER to Arduino
+void FTNoIR_Tracker::notifyCenter() {
+ if (SerialPort!=NULL) {
+ if (SerialPort->isOpen() ) {
+ SerialPort->putChar('C');
+ }
+ }
+}
+
+
+
+//send CENTER to Arduino
+void FTNoIR_Tracker::center() {
+ if (SerialPort!=NULL) {
+ if (SerialPort->isOpen() ) {
+ SerialPort->putChar('C');
+ }
+ }
+}
+
+//send RESET to Arduino
+void FTNoIR_Tracker::reset() {
+ if (SerialPort!=NULL) {
+ if (SerialPort->isOpen() ) {
+ SerialPort->putChar('R');
+ }
+ }
+}
+
+//send command to Arduino
+void FTNoIR_Tracker::sendcmd(QString* cmd) {
+ QReadLocker locker(&rwlock);
+ QByteArray bytes;
+ if (SerialPort!=NULL) {
+ if (SerialPort->isOpen() ) {
+ bytes.append(cmd->toAscii());
+ SerialPort->write(bytes);
+ }
+ }
+}
+
+// return FPS and last status
+void FTNoIR_Tracker::get_info(QString* info, int* tps ){
+ QReadLocker locker(&rwlock);
+ *tps=HAT.Code;
+ if (ListErrInf->size()>0) {
+ *info=ListErrInf->takeFirst();
+ } else {
+ *info= QString();
+ }
+}
+
+
+/** QThread run @override **/
+void FTNoIR_Tracker::run() {
+ //
+ // Read the data that was received.
+ //
+ forever {
+
+ // Check event for stop thread
+ if(::WaitForSingleObject(m_StopThread, 0) == WAIT_OBJECT_0)
+ {
+ // Set event
+ ::SetEvent(m_WaitThread);
+ qDebug() << "FTNoIR_Tracker::run() terminated run()";
+ return;
+ }
+ if (SerialPort->bytesAvailable()>=30) {
+ QWriteLocker locker(&rwlock);
+ datagram.clear();
+ datagram=SerialPort->read(30);
+ QDataStream datastream(datagram);
+ datastream>>ArduinoData;
+
+ if (ArduinoData.Begin==0xAAAA && ArduinoData.End==0x5555 ) {
+ if (ArduinoData.Code <= 1000) {
+ HAT=ArduinoData;
+ } else {
+ ListErrInf->push_back(QString::fromAscii(datagram.mid(4,24),24)) ;
+ }
+ } else {
+ SerialPort->read(1);
+ }
+ }
+ //for lower cpu load
+ usleep(10000);
+ }
+}
+
+void FTNoIR_Tracker::Initialize( QFrame *videoframe )
+{
+ qDebug() << "FTNoIR_Tracker::Initialize says: Starting ";
+
+ //
+ // Create SerialPort if they don't exist already.
+ // They must be created here, because they must be in the new thread (FTNoIR_Tracker::run())
+ //
+
+ if (SerialPort == NULL) {
+ qDebug() << "FTNoIR_Tracker::Initialize() Open SerialPort";
+ SerialPort = new QextSerialPort(sSerialPortName);
+ if (SerialPort->open(QIODevice::ReadWrite | QIODevice::Unbuffered ) == true) {
+ SerialPort->flush();
+ SerialPort->setBaudRate(BAUD115200);
+ SerialPort->setParity(PAR_NONE);
+ SerialPort->setDataBits(DATA_8);
+ SerialPort->setStopBits(STOP_1);
+ SerialPort->setFlowControl(FLOW_OFF);
+ SerialPort->setTimeout(waitTimeout);
+ SerialPort->setQueryMode(QextSerialPort::EventDriven); //Polling
+ // Send START to arduino
+ SerialPort->putChar('S');
+ }
+ else {
+ QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to open SerialPort",QMessageBox::Ok,QMessageBox::NoButton);
+ delete SerialPort;
+ SerialPort = NULL;
+ }
+ }
+ return;
+}
+
+void FTNoIR_Tracker::StartTracker( HWND parent_window )
+{
+ start( QThread::TimeCriticalPriority );
+ return;
+}
+
+void FTNoIR_Tracker::StopTracker( bool exit )
+{
+ //
+ // OK, the thread is not stopped, doing this. That might be dangerous anyway...
+ //
+ if (exit || !exit) return;
+ return;
+}
+
+//
+// Return 6DOF info
+//
+bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data)
+{
+ QReadLocker locker(&rwlock);
+ data->frame_number = HAT.Code;
+
+ if (bEnableYaw) {
+ if (bInvertYaw ) data->yaw = HAT.Gyro[iYawAxe] * -1.0f;
+ else data->yaw = HAT.Gyro[iYawAxe];
+ }
+
+ if (bEnablePitch) {
+ if (bInvertPitch)data->pitch = HAT.Gyro[iPitchAxe] * -1.0f;
+ else data->pitch = HAT.Gyro[iPitchAxe];
+ }
+
+ if (bEnableRoll) {
+ if (bInvertRoll) data->roll = HAT.Gyro[iRollAxe] * -1.0f;
+ else data->roll = HAT.Gyro[iRollAxe];
+ }
+
+ if (bEnableX) {
+ if (bInvertX) data->x = HAT.Acc[iXAxe]* -1.0f;
+ else data->x = HAT.Acc[iXAxe];
+ }
+
+ if (bEnableY) {
+ if (bInvertY) data->y = HAT.Acc[iYAxe]* -1.0f;
+ else data->y = HAT.Acc[iYAxe];
+ }
+
+ if (bEnableZ) {
+ if (bInvertZ) data->z = HAT.Acc[iZAxe]* -1.0f;
+ else data->z = HAT.Acc[iZAxe];
+ }
+
+ return true;
+}
+
+
+
+//
+// Apply modification Settings
+//
+void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){
+ qDebug()<<"Tracker:: Applying settings";
+
+ QReadLocker locker(&rwlock);
+ 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;
+
+
+ iRollAxe= settings.RollAxe;
+ iPitchAxe= settings.PitchAxe;
+ iYawAxe= settings.YawAxe;
+ iXAxe= settings.XAxe;
+ iYAxe= settings.YAxe;
+ iZAxe= settings.ZAxe;
+}
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+// 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.
+#pragma comment(linker, "/export:GetTracker=_GetTracker@0")
+
+FTNOIR_TRACKER_BASE_EXPORT ITrackerPtr __stdcall GetTracker()
+{
+ return new FTNoIR_Tracker;
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-12 17:18:48 +0000