diff options
Diffstat (limited to 'ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp')
| -rw-r--r-- | ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp | 463 |
1 files changed, 0 insertions, 463 deletions
diff --git a/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp b/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp deleted file mode 100644 index 6fef2db0..00000000 --- a/ftnoir_tracker_hatire/ftnoir_tracker_hat.cpp +++ /dev/null @@ -1,463 +0,0 @@ -/******************************************************************************** -* 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 * -* * -* Copyright (C) 2012 FuraX49 (HAT Tracker plugins) * -* Homepage: http://hatire.sourceforge.net * -* * -* * -* 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" - -FTNoIR_Tracker::FTNoIR_Tracker() -{ - ComPort = NULL; - - HAT.Rot[0]=0; - HAT.Rot[1]=0; - HAT.Rot[2]=0; - HAT.Trans[0]=0; - HAT.Trans[1]=0; - HAT.Trans[2]=0; - - // prepare & reserve QByteArray - dataRead.resize(4096); - dataRead.clear(); - Begin.append((char) 0xAA); - Begin.append((char) 0xAA); - End.append((char) 0x55); - End.append((char) 0x55); -} - -FTNoIR_Tracker::~FTNoIR_Tracker() -{ - if (ComPort!=NULL) { - if (ComPort->isOpen() ) { - ComPort->close(); - } - delete ComPort; - ComPort=NULL; - } -} - -//send CENTER to Arduino -void FTNoIR_Tracker::notifyCenter() { - sendcmd(static_cast<QString>(settings.CmdCenter).toLatin1()); -} - -//send ZERO to Arduino -bool FTNoIR_Tracker::notifyZeroed() { - sendcmd(static_cast<QString>(settings.CmdZero).toLatin1()); - return true; -} - -//send RESET to Arduino -void FTNoIR_Tracker::reset() { - sendcmd(static_cast<QString>(settings.CmdReset).toLatin1()); -} - - -// Info SerialPort -void FTNoIR_Tracker::SerialInfo() { - QByteArray Msg; - if (ComPort!=NULL) { - if (ComPort->isOpen() ) { - Msg.append("\r\n"); - Msg.append(ComPort->portName()); - Msg.append("\r\n"); - Msg.append("BAUDRATE :"); - Msg.append(QString::number(ComPort->baudRate())); - Msg.append("\r\n"); - Msg.append("DataBits :"); - Msg.append(QString::number(ComPort->dataBits())); - Msg.append("\r\n"); - Msg.append("Parity :"); - switch (ComPort->parity()) { - case 0: Msg.append("No parity"); - break; - case 2: Msg.append("Even parity"); - break; - case 3: Msg.append("Odd parity"); - break; - case 4: Msg.append("Space parity"); - break; - case 5: Msg.append("Mark parity"); - break; - default: Msg.append("Unknown parity"); - break; - } - Msg.append("\r\n"); - Msg.append("Stop Bits :"); - switch (ComPort->stopBits()) { - Msg.append(QString::number(ComPort->stopBits())); - case 1: Msg.append("1 stop bit."); - break; - case 2: Msg.append("2 stop bits."); - break; - case 3: Msg.append("1.5 stop bits."); - break; - default: Msg.append("Unknown number of stop bit."); - break; - } - Msg.append("\r\n"); - Msg.append("Flow Control :"); - switch (ComPort->flowControl()) { - case 0: Msg.append("No flow control"); - break; - case 1: Msg.append("Hardware flow control (RTS/CTS)"); - break; - case 2: Msg.append("Software flow control (XON/XOFF)"); - break; - default: Msg.append("Unknown flow control"); - break; - } - emit sendMsgInfo(Msg); - - } - } -} - - -//send command to Arduino -void FTNoIR_Tracker::sendcmd(const QByteArray &cmd) { - QByteArray Msg; - if (cmd.length()>0) { - if (ComPort->isOpen() ) - { - ComPort->write(cmd); - if (!ComPort->waitForBytesWritten(1000)) { - emit sendMsgInfo("TimeOut in writing CMD"); - } else { - Msg.append("\r\n"); - Msg.append("SEND '"); - Msg.append(cmd); - Msg.append("'\r\n"); - } - if ( !ComPort->waitForReadyRead(1000)) { - emit sendMsgInfo("TimeOut in response to CMD") ; - } else { - emit sendMsgInfo(Msg); - } - } else { - emit sendMsgInfo("ComPort not open") ; - } - } -} - - -// return FPS -void FTNoIR_Tracker::get_info( int *tps ){ - *tps=frame_cnt; - frame_cnt=0; -} - -void FTNoIR_Tracker::SerialRead() -{ - QMutexLocker lck(&mutex); - dataRead+=ComPort->readAll(); -} - -#ifndef OPENTRACK_API -void FTNoIR_Tracker::Initialize( QFrame *videoframe ) -{ - CptError=0; - dataRead.clear(); - frame_cnt=0; - - settings.load_ini(); - applysettings(settings); - ComPort = new QSerialPort(this); - ComPort->setPortName(sSerialPortName); - if (ComPort->open(QIODevice::ReadWrite ) == true) { - connect(ComPort, SIGNAL(readyRead()), this, SLOT(SerialRead())); - if ( - ComPort->setBaudRate((QSerialPort::BaudRate)iBaudRate) - && ComPort->setDataBits((QSerialPort::DataBits)iDataBits) - && ComPort->setParity((QSerialPort::Parity)iParity) - && ComPort->setStopBits((QSerialPort::StopBits)iStopBits) - && ComPort->setFlowControl((QSerialPort::FlowControl)iFlowControl) - && ComPort->clear(QSerialPort::AllDirections) - && ComPort->setDataErrorPolicy(QSerialPort::IgnorePolicy) - ) { - // Wait init arduino sequence - for (int i = 1; i <=iDelayInit; i+=50) { - if (ComPort->waitForReadyRead(50)) break; - } - sendcmd(sCmdInit); - // Wait init MPU sequence - for (int i = 1; i <=iDelayStart; i+=50) { - if (ComPort->waitForReadyRead(50)) break; - } - - } else { - QMessageBox::warning(0,"FaceTrackNoIR Error", ComPort->errorString(),QMessageBox::Ok,QMessageBox::NoButton); - } - } - else { - QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to open ComPort",QMessageBox::Ok,QMessageBox::NoButton); - delete ComPort; - ComPort = NULL; - } - return; -} - - - -void FTNoIR_Tracker::StartTracker(HWND parent_window) -{ - // Send START cmd to IMU - sendcmd(sCmdStart); - // Wait start MPU sequence - for (int i = 1; i <=iDelaySeq; i+=50) { - if (ComPort->waitForReadyRead(50)) break; - } - return; -} - - -void FTNoIR_Tracker::StopTracker( bool exit ) -{ - QByteArray Msg; - if (sCmdStop.length()>0) { - if (ComPort->isOpen() ) - { - ComPort->write(sCmdStop); - if (!ComPort->waitForBytesWritten(1000)) { - emit sendMsgInfo("TimeOut in writing CMD"); - } else { - Msg.append("\r\n"); - Msg.append("SEND '"); - Msg.append(sCmdStop); - Msg.append("'\r\n"); - } - emit sendMsgInfo(Msg); - } - } - // OK, the thread is not stopped, doing this. That might be dangerous anyway... - // - if (exit || !exit) return; - return; -} - -#else -void FTNoIR_Tracker::StartTracker(QFrame*) -{ - static const int databits_lookup[] = { - QSerialPort::Data5, - QSerialPort::Data6, - QSerialPort::Data7, - QSerialPort::Data8, - QSerialPort::UnknownDataBits - }; - - struct Local { - static int idx(int max, int value) - { - if (value < 0) - return 0; - if (max > value) - return value; - return max - 1; - } - }; - - static const int parity_lookup[] = { - QSerialPort::NoParity, - QSerialPort::EvenParity, - QSerialPort::OddParity, - QSerialPort::SpaceParity, - QSerialPort::MarkParity, - QSerialPort::UnknownParity - }; - - static const int stopbits_lookup[] = { - QSerialPort::OneStop, - QSerialPort::OneAndHalfStop, - QSerialPort::TwoStop, - QSerialPort::UnknownStopBits - }; - - static const int flowctl_lookup[] = { - QSerialPort::NoFlowControl, - QSerialPort::HardwareControl, - QSerialPort::SoftwareControl, - }; - - static const int baudrate_lookup[] = { - QSerialPort::Baud1200, - QSerialPort::Baud2400, - QSerialPort::Baud4800, - QSerialPort::Baud9600, - QSerialPort::Baud19200, - QSerialPort::Baud38400, - QSerialPort::Baud57600, - QSerialPort::Baud115200, - QSerialPort::UnknownBaud - }; - - CptError=0; - dataRead.clear(); - frame_cnt=0; - ComPort = new QSerialPort(this); - { - ComPort->setPortName(QSerialPortInfo::availablePorts().value(settings.SerialPortName).portName()); - } - if (ComPort->open(QIODevice::ReadWrite ) == true) { - connect(ComPort, SIGNAL(readyRead()), this, SLOT(SerialRead())); - if ( - ComPort->setBaudRate(baudrate_lookup[Local::idx(8, settings.pBaudRate)]) - && ComPort->setDataBits((QSerialPort::DataBits)databits_lookup[Local::idx(4, settings.pDataBits)]) - && ComPort->setParity((QSerialPort::Parity)parity_lookup[Local::idx(5, settings.pParity)]) - && ComPort->setStopBits((QSerialPort::StopBits)stopbits_lookup[Local::idx(3, settings.pStopBits)]) - && ComPort->setFlowControl((QSerialPort::FlowControl)flowctl_lookup[Local::idx(3, settings.pFlowControl)]) - && ComPort->clear(QSerialPort::AllDirections) - && ComPort->setDataErrorPolicy(QSerialPort::IgnorePolicy) - ){ - // Wait init arduino sequence - for (int i = 1; i <=settings.DelayInit; i+=50) { - if (ComPort->waitForReadyRead(50)) break; - } - sendcmd(static_cast<QString>(settings.CmdInit).toLatin1()); - // Wait init MPU sequence - for (int i = 1; i <=settings.DelayStart; i+=50) { - if (ComPort->waitForReadyRead(50)) break; - } - // Send START cmd to IMU - sendcmd(static_cast<QString>(settings.CmdStart).toLatin1()); - - // Wait start MPU sequence - for (int i = 1; i <=settings.DelaySeq; i+=50) { - if (ComPort->waitForReadyRead(50)) break; - } - } else { - QMessageBox::warning(0,"FaceTrackNoIR Error", ComPort->errorString(),QMessageBox::Ok,QMessageBox::NoButton); - } - } - else { - QMessageBox::warning(0,"FaceTrackNoIR Error", "Unable to open ComPort",QMessageBox::Ok,QMessageBox::NoButton); - delete ComPort; - ComPort = NULL; - } - return; - -} -#endif - - -// -// Return 6DOF info -// -#ifdef OPENTRACK_API -#define THeadPoseData double -#endif - -void FTNoIR_Tracker::GetHeadPoseData(THeadPoseData *data) -{ - QMutexLocker lck(&mutex); - while (dataRead.length()>=30) { - if ((dataRead.startsWith(Begin) && ( dataRead.mid(28,2)==End )) ) { // .Begin==0xAAAA .End==0x5555 - QDataStream datastream(dataRead.left(30)); - if (settings.BigEndian) datastream.setByteOrder(QDataStream::BigEndian ); - else datastream.setByteOrder(QDataStream::LittleEndian ); - datastream>>ArduinoData; - frame_cnt++; - if (ArduinoData.Code <= 1000) { - HAT=ArduinoData; - } else { - emit sendMsgInfo(dataRead.mid(4,24)) ; - } - dataRead.remove(0,30); - } else { - // resynchro trame - int index = dataRead.indexOf(Begin); - if (index==-1) { - index=dataRead.length(); - } - emit sendMsgInfo(dataRead.mid(0,index)) ; - dataRead.remove(0,index); - CptError++; - } - } - - if (CptError>50) { - emit sendMsgInfo("Can't find HAT frame") ; - CptError=0; - return; - } - data[frame_cnt] = (long) HAT.Code; - - struct Fun { - static int clamp3(int foo) - { - if (foo > 2) - return 2; - if (foo < 0) - return 0; - return foo; - } - }; - - if (settings.EnableYaw) { - if (settings.InvertYaw) data[Yaw] = (double) HAT.Rot[Fun::clamp3(settings.YawAxe)] * -1.0f; - else data[Yaw] = (double) HAT.Rot[Fun::clamp3(settings.YawAxe)]; - } - - if (settings.EnablePitch) { - if (settings.InvertPitch) data[Pitch] = (double) HAT.Rot[Fun::clamp3(settings.PitchAxe)] * -1.0f; - else data[Pitch] = (double) HAT.Rot[Fun::clamp3(settings.InvertPitch)]; - } - - if (settings.EnableRoll) { - if (settings.InvertRoll) data[Roll] = (double) HAT.Rot[Fun::clamp3(settings.RollAxe)] * -1.0f; - else data[Roll] = (double) HAT.Rot[Fun::clamp3(settings.RollAxe)]; - } - - if (settings.EnableX) { - if (settings.InvertX) data[TX] =(double) HAT.Trans[Fun::clamp3(settings.XAxe)]* -1.0f; - else data[TX] = HAT.Trans[Fun::clamp3(settings.XAxe)]; - } - - if (settings.EnableY) { - if (settings.InvertY) data[TY] =(double) HAT.Trans[Fun::clamp3(settings.YAxe)]* -1.0f; - else data[TY] = HAT.Trans[Fun::clamp3(settings.YAxe)]; - } - - if (settings.EnableZ) { - if (settings.InvertZ) data[TZ] = HAT.Trans[Fun::clamp3(settings.ZAxe)]* -1.0f; - else data[TZ] = HAT.Trans[Fun::clamp3(settings.ZAxe)]; - } -} - -void FTNoIR_Tracker::applysettings(const TrackerSettings& settings){ - QMutexLocker lck(&mutex); - settings.b->reload(); -} - -#ifdef OPENTRACK_API -extern "C" FTNOIR_TRACKER_BASE_EXPORT ITracker* CALLING_CONVENTION GetConstructor() -#else -#pragma comment(linker, "/export:GetTracker=_GetTracker@0") -FTNOIR_TRACKER_BASE_EXPORT ITrackerPtr __stdcall GetTracker() -#endif -{ - return new FTNoIR_Tracker; -} |