OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

FastTrakModule.cxx

Go to the documentation of this file.
00001 /* ========================================================================
00002  * Copyright (c) 2006,
00003  * Institute for Computer Graphics and Vision
00004  * Graz University of Technology
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are
00009  * met:
00010  *
00011  * Redistributions of source code must retain the above copyright notice,
00012  * this list of conditions and the following disclaimer.
00013  *
00014  * Redistributions in binary form must reproduce the above copyright
00015  * notice, this list of conditions and the following disclaimer in the
00016  * documentation and/or other materials provided with the distribution.
00017  *
00018  * Neither the name of the Graz University of Technology nor the names of
00019  * its contributors may be used to endorse or promote products derived from
00020  * this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
00023  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
00024  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00025  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
00026  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00028  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00029  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00030  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00031  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033  * ========================================================================
00034  * PROJECT: OpenTracker
00035  * ======================================================================== */
00043 /* ======================================================================= */
00044 
00045 // this will remove the warning 4786
00046 #include "../tool/disable4786.h"
00047 #include "../tool/OT_ACE_Log.h"
00048 
00049 #include <ace/Log_Msg.h>
00050 
00051 #include "FastTrakSource.h"
00052 #include "FastTrakModule.h"
00053 #include "../misc/serialcomm.h"
00054 
00055 #include <stdio.h>
00056 #include <string.h>
00057 #include <iostream>
00058 
00059 #include <ace/Log_Msg.h>
00060 
00061 
00062 #ifndef OT_NO_FASTTRACK_SUPPORT
00063 
00064 
00065 namespace ot {
00066 
00067     const int FASTTRAK = 1;
00068     const int ISOTRAK = 2;
00069 
00070     // constructor method.
00071     FastTrakModule::FastTrakModule() : ThreadModule(), NodeFactory(), stations( NULL )
00072     {
00073     }
00074 
00075     // Destructor method, clears nodes member.
00076     FastTrakModule::~FastTrakModule()
00077     {
00078     if( stations != NULL )
00079             delete[] stations;
00080         nodes.clear();
00081     }
00082 
00083     // initializes the tracker module. 
00084     void FastTrakModule::init(StringTable& attributes,  ConfigNode * localTree)
00085     {
00086         int num;
00087         // getting number of stations parameter
00088         num = sscanf( attributes.get("stations").c_str(), " %i", &numberOfStations);
00089         if( num != 1 )
00090         {
00091             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FastTrakModule : can't read attribute\"stations\"\n")));
00092             exit(-1);
00093         }
00094         if (numberOfStations < 1)
00095         {
00096             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FastTrakModule : attribute \"stations\" invalid\n")));
00097             exit(-1);
00098         }
00099 
00100         stations = new tmpStationEvent[numberOfStations];
00101 
00102         if (attributes.get("type").compare("fasttrak") == 0)
00103             trackerType = FASTTRAK;
00104         else if (attributes.get("type").compare("isotrak") == 0)
00105             trackerType = ISOTRAK;
00106         else
00107         {
00108             LOG_ACE_ERROR("ot:FastTrakModule : unknown trackertype %s\n", attributes.get("type").c_str());
00109             exit(-1);
00110         }
00111 
00112         strncpy( port.pathname, attributes.get("device").c_str(), 255 );
00113 
00114         hemisphere = attributes.get("hemisphere");
00115         referenceFrame = attributes.get("reference-frame");
00116         initString = attributes.get("init-string");
00117     
00118         ThreadModule::init( attributes, localTree );
00119     LOG_ACE_INFO("ot:FastTrakModule : initialized !\nusing tracker protocol for %s\n", attributes.get("type").c_str());
00120     }
00121 
00122     // This method is called to construct a new Node 
00123     Node * FastTrakModule::createNode( const std::string& name,  StringTable& attributes)
00124     {
00125         if( name.compare("FastTrakSource") == 0 )
00126         {
00127             int num, number;
00128             num = sscanf( attributes.get("number").c_str(), " %i", &number );
00129             if( num != 1 )
00130             {
00131                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FastTrakModule : error reading FastTrakSource number !\n")));
00132                 return NULL;
00133             }
00134             if ((number < 0) || (number > numberOfStations-1))
00135             {
00136                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FastTrakModule : number out of range 0 to %d\n"), numberOfStations-1));
00137                 return NULL;
00138             }
00139             FastTrakSource * source = new FastTrakSource(number);
00140             nodes.push_back( source );
00141 
00142             ACE_DEBUG((LM_INFO, ACE_TEXT("ot:Build FastTrakSource for %d\n"), number));
00143             return source;
00144         }
00145         return NULL;
00146     }
00147  
00148     // starts the module and opens all the devices etc.   
00149     void FastTrakModule::start()
00150     {
00151         if( isInitialized() == 1 )
00152         {
00153             SerialParams params;
00154             initSerialParams( &params );
00155             strcpy(params.pathname, port.pathname);
00156             params.baudrate = 115200;
00157             params.parity = 0;
00158             params.bits = 8;
00159             params.sbit = 1;
00160             params.hwflow = 1;
00161             params.swflow = 0;
00162             params.blocking = 0;
00163 
00164             if( openSerialPort( &port, &params ) < 0 )
00165             {
00166                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FastTrakModule : error opening port\n")));
00167                 initialized = 0;
00168                 return;
00169             }
00170 
00171             ThreadModule::start();
00172         }
00173     }
00174 
00175     // runs the whole initialization sequence
00176     int FastTrakModule::initFastTrak()
00177     {
00178         char buffer[256];
00179 
00180         // toggle reset
00181         //setRTSSerialPort( &port, 1);
00182         //OSUtils::sleep(100);
00183         //setRTSSerialPort( &port, 0);    
00184 
00185         // ping 
00186         static char *PingString;
00187         if (trackerType == FASTTRAK)
00188             PingString = "\rP";
00189         else if (trackerType == ISOTRAK)
00190             PingString = "P";
00191     
00192     ACE_DEBUG((LM_INFO, ACE_TEXT("ot:pinging tracker")));
00193         int ping = 0;
00194         int pong = 0;
00195         int dr;
00196         while (ping < 5)
00197         {
00198             ping++;
00199             ACE_DEBUG((LM_INFO, ACE_TEXT(".")));
00200             writetoSerialPort(&port, PingString, strlen(PingString));
00201             pong = 0;
00202             while (((dr = readfromSerialPort( &port, buffer, 255)) <= 0) && (pong < 100))
00203             {
00204                 OSUtils::sleep(10);
00205                 pong++;
00206             }
00207             if (pong < 100) 
00208             {
00209                 buffer[dr]='\0';
00210                 if (strstr(buffer,"0") != NULL)
00211                     break;
00212             }
00213         }    
00214     ACE_DEBUG((LM_INFO, ACE_TEXT("\n")));
00215     
00216         if (ping == 5)
00217         {        
00218             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FastTrakModule : can't ping tracker\n")));
00219             return -1;
00220         }
00221 
00222         // init sequence
00223 
00224         // Continous mode off
00225         writetoSerialPort(&port,"\rc\r", 3); 
00226     
00227         char init_string[256];
00228         int p = -1;
00229         while(1)
00230             if ((p = initString.find("\\r")) > -1)
00231                 initString.replace(p,2,"\r",1);
00232             else
00233                 break;
00234         strncpy(init_string, initString.c_str(), 255);
00235         writetoSerialPort(&port, init_string, strlen(init_string));
00236 
00237         for (int i=0; i<numberOfStations; i++)
00238         {
00239             // set hemisphere
00240             sprintf(buffer,"H%d,%s\r",i+1,hemisphere.c_str());
00241             writetoSerialPort(&port, buffer, strlen(buffer));
00242             // set reference frame
00243             sprintf(buffer,"R%d\rA%d,%s\r",i+1,i+1,referenceFrame.c_str());
00244             writetoSerialPort(&port, buffer, strlen(buffer));
00245             // set preferred output
00246             if (trackerType == FASTTRAK)
00247                 // always use quaternions (14 Bytes)
00248                 sprintf(buffer,"O%d,18,20\r",i+1);
00249             else if (trackerType == ISOTRAK)
00250                 // always use quaternions (49 Bytes)
00251                 sprintf(buffer,"O2,11,1\r");
00252 
00253             writetoSerialPort(&port, buffer, strlen(buffer));
00254         }    
00255 
00256         // draining input buffer of junk
00257         while (readfromSerialPort( &port, buffer, 255) > 0)
00258             ;
00259 
00260         // Continous mode on
00261         writetoSerialPort(&port, "u\rC\r", 4);
00262 
00263         // --- init end
00264 
00265         return 0;
00266     }
00267 
00268     // closes FastTrakModule, closing the serial port
00269     void FastTrakModule::close()
00270     {
00271         // stop thread
00272         lock();
00273         stop = 1;
00274         unlock();
00275 
00276         // close serial ports
00277         if( isInitialized() == 1 )
00278         {
00279             closeSerialPort(&port);
00280         }
00281     }
00282 
00283     // this method is the code executed in its own thread
00284     void FastTrakModule::run()
00285     {
00286         // return value of parser
00287         int stationNr = -1;
00288         // buffer for a data record
00289         char inputBuffer[50];
00290 
00291         // data buffer for incoming data
00292         const int maxCyclesWithoutData = 50;
00293         char readBuffer[128];
00294         int bytesRead = 0;
00295         int noData = 0;
00296     
00297         while(1)
00298         {
00299             if (initFastTrak() == -1)
00300                 continue;
00301 
00302             noData = 0;
00303             stationNr = -1;
00304 
00305             while(1)
00306             {
00307                 lock();
00308                 if( stop == 1 )
00309                 {           
00310                     unlock();
00311                     break;
00312                 } else { 
00313                     unlock();
00314                 }
00315                 if( (bytesRead = readfromSerialPort( &port, readBuffer, 128 )) < 1 )
00316                     noData++;
00317                 else
00318                     noData = 0;
00319             
00320                 for (int j=0; j<bytesRead; j++)
00321                 {
00322                     if (trackerType == FASTTRAK)
00323                         stationNr = parseRecordFT(readBuffer[j], inputBuffer);
00324                     else if (trackerType == ISOTRAK)
00325                         stationNr = parseRecordIT(readBuffer[j], inputBuffer);
00326                     if (stationNr > -1)
00327                     {
00328                         // we got a full buffer, set the data of the addressed station
00329                         lock();
00330                         convert(stationNr, inputBuffer); 
00331                         unlock();
00332                     }
00333                     else if (stationNr == -2)
00334                         break;
00335                 }
00336                 // if to much junk was read or no data was read, 
00337                 // try to init tracker again
00338                 if ((stationNr == -2) || (noData > maxCyclesWithoutData))
00339                     break;
00340                 else
00341                     OSUtils::sleep(10); 
00342             } // data processing loop
00343         
00344             if (stationNr == -2)
00345                 ACE_DEBUG((LM_WARNING, ACE_TEXT("ot:FastTrakModule: too much junk received.\n")));
00346             else
00347                 ACE_DEBUG((LM_WARNING, ACE_TEXT("ot:FastTrakModule: no data received.\n")));
00348             ACE_DEBUG((LM_INFO, ACE_TEXT("ot:FastTrakModule: trying to reinitialize tracker ...\n")));
00349 
00350         } // reinitialization loop
00351     }
00352 
00353     // pushes events into the tracker tree
00354     void FastTrakModule::pushEvent()
00355     {
00356         FastTrakSource *source;
00357 
00358         if( isInitialized() == 1 )
00359         {
00360             for( NodeVector::iterator it = nodes.begin(); it != nodes.end(); it++ )
00361             {
00362                 source = (FastTrakSource *) *it;
00363                 lock();
00364                 if (stations[source->station].newVal == 1)
00365                 {
00366                     source->event = stations[source->station].event;
00367                     stations[source->station].newVal = 0;
00368                     unlock();
00369                     source->event.timeStamp();
00370                     source->updateObservers(source->event);
00371                 }
00372                 else
00373                     unlock();
00374             }
00375         }
00376     }
00377 
00378 
00379     // events of parser
00380     const int tsStart=0;
00381     const int tsStationNumber=1;
00382     const int tsSpace=2;
00383     const int tsSyncBit=3;
00384     const int tsGetRec=4;
00385 
00386     int FastTrakModule::parseRecordFT(char c, char *inputBuffer)
00387     {
00388         static int event=tsStart;  // event variable of parser
00389         static int stNr = -1;  // event variable of parser
00390         static int pos = 0; // number of the current position in the buffer
00391         static int junk = 0;
00392 
00393         int rc = -1;
00394         int n = numberOfStations;
00396         int numOfBytes = 14;    
00397 
00398         junk++;
00399         // reset variables
00400         if (junk > 128)
00401         {
00402             event=tsStart;  // event variable of parser
00403             stNr = -1;
00404             pos = 0; // number of the current position in the buffer
00405             junk = 0;
00406             return -2;
00407         }
00408 
00409         switch (event)
00410         {
00411             case tsStart:
00412                 if (c=='0')
00413                 {              // Record Type "0", possible start of output record
00414                     (event)=tsStationNumber;
00415                 }
00416                 break;
00417 
00418             case tsStationNumber:
00419                 stNr=c-'1';
00420                 if ((stNr>=0) && (stNr<n))
00421                 {                                                // station number
00422                     (event)=tsSpace;
00423                 } else (event)=tsStart;
00424                 break;
00425 
00426             case tsSpace:
00427                 if ( c==' ') (event)=tsSyncBit; else (event)=tsStart;
00428                 break;
00429 
00430             case tsSyncBit:
00431                 pos=0;
00432                 if ((c&0x80)!=0)        // expecting to find a byte with hibit set
00433                 {
00434                     (event)=tsGetRec;
00435                     inputBuffer[pos++]=(c&0x7f);
00436                 } else { (event)=tsStart; }
00437                 break;
00438 
00439             case tsGetRec:
00440                 if ((c&0x80)!=0) { (event)=tsStart; } else
00441                 {
00442                     inputBuffer[pos++]=c;
00443                     if (pos == numOfBytes)
00444                     {                       // got one whole data record in input buffer
00445                         (event)=tsStart;
00446                         junk = 0;
00447                         rc = stNr;
00448                     }
00449                 }
00450         }
00451 
00452         return rc;
00453     }
00454 
00455     int FastTrakModule::parseRecordIT(char c, char *inputBuffer)
00456     {
00457         static int event=tsStart;  // event variable of parser
00458         static int stNr = -1;  // event variable of parser
00459         static int pos = 0; // number of the current position in the buffer
00460         static int junk = 0;
00461 
00462         int rc = -1;
00463         int n = numberOfStations;
00464         // number of bytes of the tracker's output record     
00465         int numOfBytes = 49;    
00466     
00467         junk++;
00468         // reset variables
00469         if (junk > 512)
00470         {
00471             event=tsStart;  // event variable of parser
00472             stNr = -1;
00473             pos = 0; // number of the current position in the buffer
00474             junk = 0;
00475             return -2;
00476         }
00477 
00478         switch (event)
00479         {
00480             case tsStart:
00481                 // Record Type "0", possible start of output record
00482                 if (c=='0')
00483                     (event)=tsStationNumber;
00484                 break;
00485 
00486             case tsStationNumber:
00487                 // station number
00488                 stNr=c-'1';
00489                 if ((stNr>=0) && (stNr<n))
00490                     (event)=tsSpace;
00491                 else (event)=tsStart;
00492                 break;
00493 
00494             case tsSpace:
00495                 if (( c==' ') || (( c=='*' )  || ( c=='@' )))
00496                 {
00497                     if ( c=='*' ) setButtonIT(stNr, 0);
00498                     if ( c=='@' ) setButtonIT(stNr, 1);
00499                     (event)=tsGetRec;
00500                 } else (event)=tsStart;
00501                 break;
00502 
00503             case tsGetRec:
00504                 // got one whole data record in buffer
00505                 if (pos == numOfBytes)
00506                 {
00507                     if (c=='\r')
00508                     {
00509                         (event)=tsStart;
00510                         pos = 0;
00511                         junk = 0;
00512                         rc = stNr;
00513                     }
00514                     else
00515                     {
00516                         (event)=tsStart;
00517                         pos = 0;
00518                     }
00519                 }
00520                 else
00521                     inputBuffer[pos++]=c;
00522                 break;
00523         }
00524 
00525         return(rc);
00526     }
00527 
00528 
00529     /*** function getBinary
00530      *
00531      * Purpose: Returns the two bytes from the buffer as a Fastrak 16 bit integer
00532      * In:      buffer:     Pointer to array of bytes to be converted
00533      * Returns: Fastrak 16 bit integer
00534      * Roman Rath, Zsolt Szalavari
00535      */
00536 
00537     short int getBinary(const char *buffer)
00538     {
00539         int d;
00540 
00541         d=(buffer[0]&0x7f)+(buffer[1]<<7); 
00542         if (d&0x2000) { d=(d&0x1ffff)-16384; }
00543 
00544         return d;
00545     }
00546 
00547     double getASC(const char *buffer)
00548     {
00549         double d;
00550         char ch[7];
00551     
00552         strncpy(ch, buffer,7);
00553         d=atof(ch);
00554     
00555         return(d);
00556     }
00557 
00558 
00559     /*** function buildPosition
00560      *
00561      * Purpose: Convert position bytes from Fastrak into actual position int
00562      *          values
00563      * In:      buffer:     Buffer containing bytes received from Fastrak
00564      * In/Out:  position:   Reference to a vector of floats for the position to
00565      *                      be put in
00566      * Roman Rath, Zsolt Szalavari
00567      */
00568 
00569     void buildPositionFT(char* buffer, std::vector<float>& position)
00570     {
00571         for (int i=0; i<3; i++)
00572         {
00573             position[i]=(float)(getBinary(buffer+i*2));
00574             position[i] *= 0.01f;
00575         }
00576     }
00577 
00578     void buildPositionIT(char* buffer, std::vector<float>& position)
00579     {
00580         for (int i=0; i<3; i++)
00581         {
00582             position[i]=(float)(getASC(buffer+i*7));
00583             position[i] *= 0.01f;
00584         }
00585     }
00586 
00587 
00588     /*** function buildQuaternionFT
00589      *
00590      * Purpose: Convert quaternion bytes from Fastrak into actual quaternion
00591      *          float values
00592      * In:      buffer:         Buffer containing bytes received from Fastrak
00593      * In/Out:  quaternion:     Pointer to an array of floats for the quaternion
00594      *                          to be put in
00595      * Roman Rath, Zsolt Szalavari
00596      */
00597 
00598     void buildQuaternionFT(char* buffer, std::vector<float> &quaternion)
00599     {
00600         for (int i=0; i<4; i++)
00601         {
00602             quaternion[(i+3)%4]=((float)(getBinary(buffer+i*2+6)))/8192;
00603         }
00604     
00605     }
00606 
00607     void buildQuaternionIT(char* buffer, std::vector<float> &quaternion)
00608     {
00609         for (int i=0; i<4; i++)
00610         {
00611             quaternion[(i+3)%4]=(float)(getASC(buffer+i*7+21));
00612         }
00613     }
00614 
00615 
00616     void FastTrakModule::setButtonIT(int stationNr, int button)
00617     {
00618         if (stations[stationNr].event.getButton() != button)
00619         {
00620             lock();
00621             stations[stationNr].event.getButton() = button;
00622             stations[stationNr].newVal = 1;
00623             unlock();
00624         }
00625     }
00626 
00627     // function processFastrakRecord
00628 
00629     void FastTrakModule::convert(int stationNr, char *inputBuffer)
00630     {
00631         if (trackerType == FASTTRAK)
00632         {
00633             buildPositionFT(inputBuffer, stations[stationNr].event.getPosition());
00634             buildQuaternionFT(inputBuffer,stations[stationNr].event.getOrientation());
00635         }
00636         else if (trackerType == ISOTRAK)
00637         {
00638             buildPositionIT(inputBuffer, stations[stationNr].event.getPosition());
00639             buildQuaternionIT(inputBuffer,stations[stationNr].event.getOrientation());
00640         }
00641 
00642         stations[stationNr].newVal = 1;
00643     }
00644 
00645 } // namespace ot
00646 
00647 
00648 #endif // OT_NO_FASTTRACK_SUPPORT
00649 
00650 /* 
00651  * ------------------------------------------------------------
00652  *   End of FastTrakModule.cxx
00653  * ------------------------------------------------------------
00654  *   Automatic Emacs configuration follows.
00655  *   Local Variables:
00656  *   mode:c++
00657  *   c-basic-offset: 4
00658  *   eval: (c-set-offset 'substatement-open 0)
00659  *   eval: (c-set-offset 'case-label '+)
00660  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00661  *   eval: (setq indent-tabs-mode nil)
00662  *   End:
00663  * ------------------------------------------------------------ 
00664  */

copyright (c) 2006 Graz University of Technology