/******************************************************************************** * 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 . * * * ********************************************************************************/ #include "ftnoir_tracker_hat.h" #include #include #include 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(); // 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; }