OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

FOBModule.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 "FOBSource.h"
00052 #include "FOBModule.h"
00053 #include "../core/MathUtils.h"
00054 #include "../misc/serialcomm.h"
00055 
00056 #include <stdio.h>
00057 #include <string.h>
00058 #include <iostream>
00059 
00060 
00061 #ifndef OT_NO_FOB_SUPPORT
00062 
00063 
00064 namespace ot {
00065 
00070     class Bird {
00071     public:
00072 
00074         static const float inchesToMeters ; //= 0.0254;
00075 
00077         int number;
00078     
00080         SerialPort port;
00081 
00083         char buffer[20];
00084 
00086         int count;
00087 
00089         FOBSource * source;
00090 
00092         Event event;
00093 
00095         bool newVal;
00096 
00098         float scale;
00099 
00101         float angleAlign[3];
00102 
00104         Bird( int number_, const std::string & device_, float scale_, float * anglealign_ = NULL ) :
00105             number( number_ )
00106         {
00107             source = NULL;
00108             count = 0;
00109             newVal = false;
00110             scale = scale_ * inchesToMeters / 32767;
00111             if( anglealign_ != NULL )
00112             {
00113                 angleAlign[0] = anglealign_[0];
00114                 angleAlign[1] = anglealign_[1];
00115                 angleAlign[2] = anglealign_[2];            
00116             }
00117             else 
00118             {
00119                 angleAlign[0] = 0;
00120                 angleAlign[1] = 0;
00121                 angleAlign[2] = 0;
00122             }
00123             event.getConfidence() = 1;
00124             strncpy( port.pathname, device_.c_str(), 255 );
00125         }
00126     
00129         inline void convert();
00130 
00136         inline void convert( const char * buffer );
00137 
00147         inline int parse( const char * buffer, int len, int framesize );
00148     
00151         inline int open();
00152 
00158         inline int write( const char * data, int count );
00159 
00165         inline int read( char * data, int count );
00166 
00169         inline int close();
00170 
00174         inline int reset();
00175 
00176         // some bird protocol stuff
00177         inline int setGroupMode( const bool value );
00178 
00179         inline int getErrorCode();
00180 
00181         inline int autoConfig( const int number );
00182 
00183         inline void sleep();
00184 
00185         inline void setReportMode( const int toBird = -1 );
00186 
00187         inline void setScale( const int scale, const int toBird = -1 );
00188 
00189         inline void setXYZFrame( const bool useFrame, const int toBird = -1 );
00190 
00191         inline void setHemisphere( const FOBModule::Hemisphere hemisphere, const int toBird = -1 );
00192 
00193         inline void setAngleAlign( const float * angles, const int toBird = -1 );
00194 
00195         inline void setReferenceFrame( const float * angles, const int toBird = -1 );
00196 
00197         inline int sendReset();
00198     };
00199 
00200 
00201     // constructor method.
00202     FOBModule::FOBModule() : ThreadModule(), NodeFactory()
00203     {
00204     }
00205 
00206     // Destructor method, clears nodes member.
00207     FOBModule::~FOBModule()
00208     {
00209         std::map<int, Bird *>::iterator it;
00210         for( it = birds.begin(); it != birds.end(); it++)
00211         {
00212             delete it->second;
00213         }
00214         birds.clear();
00215     }
00216 
00217     // initializes the tracker module. 
00218     void FOBModule::init(StringTable& attributes,  ConfigNode * localTree)
00219     {
00220         // checking mode parameter
00221         if( attributes.get("mode").compare("multi") == 0 )
00222             mode = MULTI;
00223         else
00224             mode = SINGLE;
00225     
00226         // getting master parameter
00227         if( attributes.get("master", &master ) != 1 )
00228         {        
00229             LOG_ACE_ERROR("ot:error in master parameter %s\n", attributes.get("master").c_str());
00230             return;
00231         }
00232     
00233         // getting scale
00234         if( attributes.get("scale", &scale ) != 1 )
00235             scale = 36;
00236     
00237         // getting erc transmitter
00238         if( attributes.get("transmitter", &transmitter ) != 1 )
00239             transmitter = -1;
00240         else        
00241             scale = 144;
00242 
00243         // getting hemisphere
00244         if( attributes.containsKey("hemisphere"))
00245         {    
00246             if( attributes.get("hemisphere").compare("forward") == 0 )
00247                 hemisphere = FORWARD;
00248             else if ( attributes.get("hemisphere").compare("rear") == 0 )
00249                 hemisphere = REAR;
00250             else if ( attributes.get("hemisphere").compare("upper") == 0 )
00251                 hemisphere = UPPER;
00252             else if ( attributes.get("hemisphere").compare("lower") == 0 )
00253                 hemisphere = LOWER;
00254             else if ( attributes.get("hemisphere").compare("left") == 0 )
00255                 hemisphere = LEFT;
00256             else if ( attributes.get("hemisphere").compare("right") == 0 )
00257                 hemisphere = RIGHT;
00258         }       
00259 
00260         // getting reference frame    
00261         if( attributes.get("referenceframe", referenceframe, 3 ) != 3 )
00262         {
00263             referenceframe[0] = 0;
00264             referenceframe[1] = 0;
00265             referenceframe[2] = 0;
00266         }
00267 
00268         if( attributes.get("xyzframe").compare("false") == 0 )
00269             useXYZFrame = false;
00270         else
00271             useXYZFrame = true;
00272 
00273         // parsing birds
00274         for( unsigned int i = 0; i < localTree->countChildren(); i++ )
00275         {
00276             ConfigNode * child = (ConfigNode *)localTree->getChild(i);
00277             StringTable & childAttr = child->getAttributes();
00278             int number;
00279             if( childAttr.get("number", &number ) != 1 )
00280             {
00281                 LOG_ACE_ERROR("ot:FOBModule : error parsing Bird %d with number %s\n", childAttr.get("number").c_str());
00282                 continue;
00283             }
00284 
00285             // getting anglealign
00286             float angles[3];
00287             if( attributes.get("anglealign", angles, 3 ) != 3 )
00288             {
00289                 angles[0] = 0;
00290                 angles[1] = 0;
00291                 angles[2] = 0;
00292             }
00293 
00294             Bird * bird = new Bird( number, childAttr.get("device"), scale, angles );        
00295             birds[number] = bird;        
00296             LOG_ACE_INFO("ot:created bird %d on dev %s\n", number, childAttr.get("device").c_str());
00297         }
00298     
00299         if( birds.find(master) == birds.end())
00300         {        
00301             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error no master bird %d\n"), master));
00302             return;
00303         }
00304         ThreadModule::init( attributes, localTree );
00305     }
00306 
00307     // This method is called to construct a new Node 
00308     Node * FOBModule::createNode( const std::string& name,  StringTable& attributes)
00309     {
00310         if( name.compare("FOBSource") == 0 )
00311         {
00312             int number;
00313             if( attributes.get("number", &number ) != 1 )
00314             {            
00315                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error reading FOBSource number !\n")));
00316                 return NULL;
00317             }
00318             if( birds.find(number) == birds.end())
00319             {            
00320                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : no Bird with number %d present\n"), number));
00321                 return NULL;
00322             }
00323             FOBSource * source = new FOBSource();
00324             birds[number]->source = source;        
00325             ACE_DEBUG((LM_INFO, ACE_TEXT("ot:Build FOBSource for %d\n"), number));
00326             return source;
00327         }
00328         return NULL;
00329     }
00330  
00331     // starts the module and opens all the devices etc.   
00332     void FOBModule::start()
00333     {
00334         if( isInitialized() == 1 )
00335         {
00336             if( mode == SINGLE )
00337             {
00338                 if( birds[master]->open() < 0 )
00339                 {
00340                     ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error opening %d port for master %d\n"), master, birds[master]->number));
00341                     initialized = 0;
00342                     return;
00343                 }
00344             } else {
00345                 std::map<int, Bird *>::iterator it;
00346                 for( it = birds.begin(); it != birds.end(); it++ )
00347                 {
00348                     if( it->second->open() < 0 )
00349                     {                    
00350                         ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error opening port for %d\n"), it->first));
00351                         initialized = 0;
00352                         return;
00353                     }
00354                 }           
00355             }
00356             ThreadModule::start();
00357         }
00358     }
00359 
00360     // runs the whole initialization sequence
00361     int FOBModule::initFoB()
00362     {
00363         Bird * masterBird = birds[master];
00364         int result;
00365 
00366         // reset birds
00367         if((result = resetBirds()) != 0 )
00368         {        
00369             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error reseting birds %d\n"), result));
00370             return result;
00371         }
00372         OSUtils::sleep(300);
00373 
00374         if((result = masterBird->setGroupMode( false )) != 0 )
00375         {        
00376             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error clear group mode %d\n"), result));
00377             return result;
00378         }
00379         OSUtils::sleep(300);
00380 
00381         if((result = masterBird->autoConfig((transmitter > birds.rbegin()->first)?(transmitter):(birds.rbegin()->first))) 
00382            != 0 )
00383         {        
00384             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error clear group mode %d\n"), result));
00385             return result;
00386         }
00387         OSUtils::sleep(300);
00388 
00389         if((result = setReportMode()) != 0 )
00390         {        
00391             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error setting report mode %d\n"), result));
00392             return result;
00393         }
00394     
00395         // set all kinds of other parameters here
00396         setHemisphere();
00397 
00398         setScale();
00399 
00400         setAngleAlign();
00401 
00402         setReferenceFrame();
00403     
00404         setXYZFrame();
00405 
00406         if((result = setNextTransmitter()) != 0 )
00407         {        
00408             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error setting transmitter %d\n"), result));
00409             // return result;
00410         }
00411         OSUtils::sleep(500);
00412 
00413         if( mode == SINGLE )
00414         {
00415             if((result = masterBird->setGroupMode( true )) != 0 )
00416             {       
00417                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error setting group mode %d\n"), result));
00418                 // return result;
00419             }
00420         }
00421         OSUtils::sleep(500);
00422         if((result = startStreamMode()) != 0 )
00423         {        
00424             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule : error starting data streaming %d\n"), result));
00425             //return result;
00426         }
00427         return 0;
00428     }
00429 
00430     // closes FOBModule, closing all serial ports
00431     void FOBModule::close()
00432     {
00433         // stop thread
00434         lock();
00435         stop = true;
00436         unlock();
00437 
00438         // close serial ports
00439         if( isInitialized() == 1 )
00440         {
00441             birds[master]->sleep();
00442             if( mode == SINGLE )
00443             {
00444                 birds[master]->close();
00445             } else {
00446                 std::map<int, Bird *>::iterator it;
00447                 for( it = birds.begin(); it != birds.end(); it++ )
00448                 {
00449                     it->second->close();
00450                 }
00451             }
00452         }
00453     }
00454 
00455     // this method is the code executed in its own thread
00456     void FOBModule::run()
00457     {
00458         char buffer[100];
00459         int len = 0;
00460         int count = 0;
00461         // the number of bytes in one data frame for group mode (single serial interface)
00462         const int GROUP_FRAME_SIZE = 15;
00463         // the number of bytes in one data frame for multi mode
00464         const int MULTI_FRAME_SIZE = 14;
00465         // timeout before we think that something is wrong, right now 1 second
00466         const double DATA_TIME_OUT = 1000 * 1;
00467         // number of allowed failures 
00468         const int MAX_FAILURES = 5;
00469 
00470         int failure = 0;
00471 
00472         initFoB();
00473 
00474         unsigned int iter = 0;    
00475         double startTime = OSUtils::currentTime();
00476         double lastDataTime = OSUtils::currentTime();
00477 
00478         if( mode == SINGLE )
00479         {
00480             Bird * bird = birds[master];
00481             int num;
00482 
00483             while(1)
00484             {
00485                 // yield to let other processes do something
00486                 OSUtils::sleep(1);
00487                 iter++;            
00488                 lock();
00489                 if( stop == true )
00490                 {           
00491                     unlock();
00492                     break;
00493                 } else { 
00494                     unlock();
00495                 }      
00496             
00497                 // test if something is wrong, if so do an init again 
00498                 if((OSUtils::currentTime() - lastDataTime) > DATA_TIME_OUT ||
00499                    failure > MAX_FAILURES )
00500                 {
00501                     initFoB();
00502                     lastDataTime = OSUtils::currentTime();
00503                     failure = 0;
00504                 }
00505 
00506                 // read in data and parse it 
00507                 if( (len = bird->read( buffer, 100 )) <= 0 )
00508                 {
00509                     if( errno != EAGAIN )
00510                     {
00511                         failure++;
00512                     }
00513                     continue;
00514                 }
00515                 lastDataTime = OSUtils::currentTime();
00516                 while( count < len )
00517                 {
00518                     // let the master bird parse the buffer
00519                     count += bird->parse( &buffer[count], len - count, GROUP_FRAME_SIZE );
00520                     if( bird->count == GROUP_FRAME_SIZE )
00521                     {
00522                         bird->count = 0;
00523                         num = bird->buffer[GROUP_FRAME_SIZE - 1];
00524                         if( birds.find(num) != birds.end())
00525                         {
00526                             lock();                    
00527                             birds[num]->convert( bird->buffer );
00528                             birds[num]->newVal = true;
00529                             unlock();                        
00530                         }
00531                     }
00532                 }
00533                 count = 0;
00534             }
00535         } else {
00536             while(1)
00537             {
00538                 // yield to let other processes do something
00539                 OSUtils::sleep(1);
00540                 iter++;
00541                 lock();
00542                 if( stop == true )
00543                 {           
00544                     unlock();
00545                     break;
00546                 } else { 
00547                     unlock();
00548                 }
00549 
00550                 // test if something is wrong, if so do an init again 
00551                 if((OSUtils::currentTime() - lastDataTime) > DATA_TIME_OUT ||
00552                    failure > MAX_FAILURES )
00553                 {
00554                     initFoB();
00555                     lastDataTime = OSUtils::currentTime();
00556                     failure = 0;
00557                 }
00558 
00559                 std::map<int,Bird *>::iterator it;
00560                 for( it = birds.begin(); it != birds.end(); it++ )
00561                 {
00562                     Bird * bird = it->second;
00563                     if( (len = bird->read( buffer, 100 )) <= 0 )
00564                     {
00565                         if( errno != EAGAIN )
00566                         {
00567                             failure++;
00568                         }                    
00569                         continue;
00570                     }
00571                     lastDataTime = OSUtils::currentTime();
00572                     while( count < len )
00573                     {
00574                         // let the bird itself parse the buffer
00575                         count += bird->parse( &buffer[count], len - count, MULTI_FRAME_SIZE );
00576                         if( bird->count == MULTI_FRAME_SIZE )
00577                         {
00578                             bird->count = 0;
00579                             lock();
00580                             bird->convert();
00581                             bird->newVal = true;
00582                             unlock();
00583                         }
00584                     }
00585                     count = 0;
00586                 }
00587             }
00588         }    
00589     ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:FOBModule Framerate %f\n"), 1000 * iter / ( OSUtils::currentTime() - startTime )));
00590     }
00591     
00592     // pushes events into the tracker tree
00593     void FOBModule::pushEvent()
00594     {
00595         if( isInitialized() == 1 )
00596         {
00597             std::map<int,Bird *>::iterator it;
00598             for( it = birds.begin(); it != birds.end(); it++ )
00599             {
00600                 lock();
00601                 if(it->second->newVal == 1 && it->second->source != NULL)
00602                 {
00603                     it->second->source->event = it->second->event;
00604                     it->second->newVal = 0;
00605                     unlock();
00606                     it->second->source->updateObservers(it->second->source->event);
00607                 } else
00608                     unlock();
00609             }
00610         }
00611     }
00612 
00613     // toggle reset on the birds
00614     int FOBModule::resetBirds()
00615     {    
00616         if( mode == SINGLE )
00617         {
00618             birds[master]->reset();
00619         }
00620         else
00621         {
00622             std::map<int, Bird *>::iterator it;
00623             for( it = birds.begin(); it != birds.end(); it++ )
00624             {
00625             it->second->reset();
00626             }
00627         }
00628         OSUtils::sleep(300);
00629         return birds[master]->sendReset();
00630     }
00631 
00632     // set report mode to POSITION/QUATERNION
00633     int FOBModule::setReportMode()
00634     { 
00635         std::map<int, Bird *>::iterator it;
00636         if( mode == MULTI )
00637         {
00638             for( it = birds.begin(); it != birds.end(); it++ )
00639             {
00640                 it->second->setReportMode();
00641             }
00642             OSUtils::sleep(600);
00643         } else {
00644             // send all birds the POSITION/QUATERNION command
00645             for( it = birds.begin(); it != birds.end(); it++ )
00646             {
00647                 birds[master]->setReportMode( it->first );
00648                 OSUtils::sleep(300);
00649             }        
00650         }
00651         return birds[master]->getErrorCode();
00652     }
00653 
00654     // Turn on ERC if present (or another transmitter :)
00655     int FOBModule::setNextTransmitter()
00656     {
00657         char buffer[2];
00658         if( transmitter != -1 )
00659         {
00660             buffer[0] = 0x30;
00661             buffer[1] = (transmitter << 4) & 0xff;
00662             birds[master]->write( buffer, 2 );
00663             OSUtils::sleep( 300 );
00664         }
00665         return birds[master]->getErrorCode();
00666     }
00667 
00668     // start STREAM mode    
00669     int FOBModule::startStreamMode()
00670     {
00671         char buffer[2];
00672 
00673         if( mode == SINGLE )
00674         {
00675             // only to master bird
00676             buffer[0] = 0xF0+birds[master]->number;
00677             buffer[1] = '@';
00678             birds[master]->write( buffer, 2 );
00679         } else {
00680             // to all
00681             buffer[0] = '@';
00682             std::map<int, Bird *>::iterator it;
00683             for( it = birds.begin(); it != birds.end(); it++ )
00684             {
00685                 it->second->write( buffer, 1 );
00686             }
00687         }
00688         OSUtils::sleep(500);
00689         // no use getting an error code :)
00690         return 0;
00691     }
00692 
00693     void FOBModule::setScale()
00694     {
00695         if( scale != 72 )
00696             return;
00697         std::map<int, Bird *>::iterator it;
00698         if( mode == SINGLE )
00699         {
00700             for( it = birds.begin(); it != birds.end(); it++ )
00701             {
00702                 birds[master]->setScale( (int)scale, it->first );
00703                 OSUtils::sleep( 300 );
00704             }
00705         }
00706         else
00707         {
00708             for( it = birds.begin(); it != birds.end(); it++ )
00709             {
00710                 it->second->setScale( (int)scale );            
00711             }
00712             OSUtils::sleep( 300 );
00713         }
00714     }
00715 
00716     void FOBModule::setAngleAlign()
00717     {
00718         std::map<int, Bird *>::iterator it;
00719         if( mode == SINGLE )
00720         {
00721             for( it = birds.begin(); it != birds.end(); it++ )
00722             {
00723                 float * angles = it->second->angleAlign;
00724                 if( angles[0] == 0 && angles[1] == 0 && angles[2] == 0 )
00725                     continue;
00726                 birds[master]->setAngleAlign( angles , it->first );
00727                 OSUtils::sleep( 300 );
00728             }
00729         }
00730         else
00731         {
00732             for( it = birds.begin(); it != birds.end(); it++ )
00733             {
00734                 float * angles = it->second->angleAlign;
00735                 if( angles[0] == 0 && angles[1] == 0 && angles[2] == 0 )
00736                     continue;
00737                 it->second->setAngleAlign( angles );            
00738             }
00739             OSUtils::sleep( 300 );
00740         }    
00741     }
00742 
00743     void FOBModule::setReferenceFrame()
00744     {
00745         if( referenceframe[0] == 0 && referenceframe[1] == 0 && referenceframe[2] == 0 )
00746             return;
00747 
00748         birds[master]->setReferenceFrame( referenceframe );
00749         OSUtils::sleep( 300 );
00750     }
00751     
00752     void FOBModule::setXYZFrame()
00753     {
00754         std::map<int, Bird *>::iterator it;
00755         if( mode == SINGLE )
00756         {
00757             for( it = birds.begin(); it != birds.end(); it++ )
00758             {
00759                 birds[master]->setXYZFrame( useXYZFrame, it->first );
00760                 OSUtils::sleep( 300 );
00761             }
00762         }
00763         else
00764         {
00765             for( it = birds.begin(); it != birds.end(); it++ )
00766             {
00767                 it->second->setXYZFrame( useXYZFrame );            
00768             }
00769             OSUtils::sleep( 300 );
00770         }
00771     }
00772 
00773     void FOBModule::setHemisphere()
00774     {
00775         std::map<int, Bird *>::iterator it;
00776         if( mode == SINGLE )
00777         {
00778             for( it = birds.begin(); it != birds.end(); it++ )
00779             {
00780                 birds[master]->setHemisphere( hemisphere, it->first );
00781                 OSUtils::sleep( 300 );
00782             }
00783         }
00784         else
00785         {
00786             for( it = birds.begin(); it != birds.end(); it++ )
00787             {
00788                 it->second->setHemisphere( hemisphere );
00789             }
00790             OSUtils::sleep( 300 );
00791         }
00792     }
00793 
00794     const float Bird::inchesToMeters = 0.0254f;
00795 
00796     // convert Bird data in buffer to OpenTracker Event format
00797     inline void Bird::convert()
00798     {
00799         convert( buffer );
00800     }
00801 
00802     inline void Bird::convert( const char * data )
00803     {
00804         int d,i;
00805         event.timeStamp();
00806 
00807         for (i=0; i<3; i++)                         // x,y,z
00808         {
00809             d=(((int)(data[i*2]&0x7f))<<2)+(((int)data[i*2+1])<<9);
00810             if (d&0x8000)
00811                 d|=0xffff0000;        // Extend signbit
00812             event.getPosition()[i] = ((float)d)*scale;
00813         }
00814         for (i=0; i<4; i++)                         // qa,qx,qy,qz
00815         {
00816             d=(((int)(data[6+2*i]&0x7f))<<2)+(((int)data[7+2*i])<<9);
00817             if (d&0x8000)
00818                 d|=0xffff0000;        // Extend signbit
00819             event.getOrientation()[(i+3)%4] = ((float)d)/0x8000; // we need qx, qy, qz, qa
00820             // so (i+3)%4 should do the
00821             // trick. scale ?????
00822         }
00823     event.getOrientation()[3] *= -1;    // for some reason we get the
00824         // coordinates inverted, therefore we invert the scalar !
00825     }
00826 
00827     inline int Bird::parse( const char * data, int len, int framesize )
00828     {
00829         int i = 0;
00830         if( count == 0 )  // we are still looking for the phasing bit
00831         {
00832             for( i = 0 ; i < len ; i++ )
00833             {
00834                 if( data[i] & 0x80 )
00835                     break;
00836             }
00837             if( i == len )  // read everything but found no phasing bit, throw it away
00838                 return len;
00839             buffer[0] = data[i];
00840             count = 1;
00841             i++;
00842         }
00843         // copy everything up to min(framesize - count, len - i) bytes from data[i] into buffer
00844         //int amount = min( framesize - count, len - i );
00845     int amount = (framesize - count) < (len - i) ? (framesize - count) : (len - i);
00846         if( amount == 0 )
00847             return i;
00848         memcpy( &buffer[count], &data[i], amount );
00849         count += amount;
00850         return i+amount;
00851     }
00852 
00853     inline int Bird::open()
00854     {
00855         SerialParams params;
00856         initSerialParams( &params );
00857         params.baudrate = 115200;
00858         params.parity = 0;
00859         params.bits = 8;
00860         params.sbit = 1;
00861         params.hwflow = 1;
00862         params.swflow = 0;
00863         params.blocking = 0;
00864         strncpy( params.pathname, port.pathname, 255 );
00865         return openSerialPort( &port, &params );
00866     }
00867 
00868     inline int Bird::write( const char * data, int count )
00869     {
00870         return writetoSerialPort( &port,(char *) data, count );
00871     }
00872 
00873     inline int Bird::read( char * data, int count )
00874     {
00875         return readfromSerialPort( &port, data, count );
00876     }
00877 
00878     inline int Bird::close()
00879     {
00880         return closeSerialPort(&port);
00881     }
00882 
00883     inline int Bird::reset()
00884     {
00885         setRTSSerialPort( &port, 1);
00886         OSUtils::sleep(100);
00887         return setRTSSerialPort( &port, 0);
00888     }
00889 
00890     inline int Bird::setGroupMode( bool value )
00891     {
00892         char buffer[3];
00893         buffer[0] = 'P';
00894         buffer[1] = 0x23;
00895         buffer[2] = (value)?(1):(0);
00896         write( buffer, 3 );
00897         OSUtils::sleep( 600 );
00898         return getErrorCode();
00899     }
00900 
00901     inline int Bird::getErrorCode()
00902     {
00903         buffer[0] = 0x4f;
00904         buffer[1] = 10;
00905         write( buffer, 2 );
00906         OSUtils::sleep(100);
00907         buffer[0] = 0;
00908         int count = 0;
00909         while( read( buffer, 1 ) < 0 && count < 10)
00910         {
00911             if( errno != EAGAIN )
00912                 return -1;  
00913             count++;
00914             OSUtils::sleep(10);
00915         }
00916         return buffer[0];
00917     }
00918 
00919     inline int Bird::autoConfig( const int number )
00920     {
00921         char buffer[3];
00922         buffer[0] = 'P';
00923         buffer[1] = 0x32;
00924         buffer[2] = number;
00925         write( buffer, 3 );
00926         OSUtils::sleep(600);
00927         return getErrorCode();
00928     }
00929 
00930     inline void Bird::sleep()
00931     {
00932         char buffer[4] = "GGG";
00933         write( buffer, 3 );
00934         OSUtils::sleep(300);    
00935     }
00936 
00937     inline void Bird::setReportMode( const int toBird )
00938     {
00939         char buffer[2];
00940         if( toBird == -1 || toBird == number )
00941         {
00942             buffer[0] = ']';
00943             write( buffer, 1 );
00944         }
00945         else 
00946         {
00947             buffer[0] = 0xF0+toBird;
00948             buffer[1] = ']';
00949             write( buffer, 2 );
00950         }
00951     }
00952 
00953     inline void Bird::setScale( const int scale, const int toBird )
00954     {
00955         char buffer[5];
00956         if( toBird == -1 || toBird == number )
00957         {
00958             buffer[0] = 'P';
00959             buffer[1] = 0x3;
00960             buffer[2] = 0;
00961             buffer[3] = (scale==72)?(1):(0);
00962             write( buffer, 4 );
00963         }
00964         else 
00965         {
00966             buffer[0] = 0xF0+toBird;
00967             buffer[1] = 'P';
00968             buffer[2] = 0x3;
00969             buffer[3] = 0;
00970             buffer[4] = (scale==72)?(1):(0);
00971             write( buffer, 5 );
00972         }
00973     }
00974 
00975     inline void Bird::setXYZFrame( const bool useFrame, const int toBird )
00976     {
00977         char buffer[4];
00978         if( toBird == -1 || toBird == number )
00979         {
00980             buffer[0] = 'P';
00981             buffer[1] = 17;
00982             buffer[2] = (useFrame)?(1):(0);
00983             write( buffer, 3 );
00984         }
00985         else 
00986         {
00987             buffer[0] = 0xF0+toBird;
00988             buffer[1] = 'P';
00989             buffer[2] = 17;
00990             buffer[3] = (useFrame)?(1):(0);
00991             write( buffer, 4 );        
00992         }
00993     }
00994 
00995     inline void Bird::setHemisphere( const FOBModule::Hemisphere hemisphere, const int toBird )
00996     {
00997         char buffer[4];
00998         if( hemisphere != FOBModule::FORWARD )
00999         {
01000             buffer[1] = 'L';
01001             switch( hemisphere )
01002             {
01003                 case FOBModule::REAR:
01004                     buffer[2] = 0x00;
01005                     buffer[3] = 0x01;
01006                     break;
01007                 case FOBModule::UPPER:
01008                     buffer[2] = 0x0c;
01009                     buffer[3] = 0x01;
01010                     break;
01011                 case FOBModule::LOWER:
01012                     buffer[2] = 0x0c;
01013                     buffer[3] = 0x00;
01014                     break;
01015                 case FOBModule::LEFT:
01016                     buffer[2] = 0x06;
01017                     buffer[3] = 0x01;
01018                     break;
01019                 case FOBModule::RIGHT:
01020                     buffer[2] = 0x06;
01021                     buffer[3] = 0x00;
01022                     break;
01023                 default:
01024                     break;
01025             }
01026             if( toBird == -1 || toBird == number )
01027             {
01028                 write( &buffer[1], 3 );
01029             }
01030             else 
01031             {
01032                 buffer[0] = 0xF0+toBird;
01033                 write( buffer, 4 );
01034             }
01035         }
01036     }
01037 
01038     inline void Bird::setAngleAlign( const float * angles, const int toBird )
01039     {
01040         char buffer[8];
01041         buffer[1] = 0x71;
01042 
01043         for( int i = 0; i < 3; i++ )    
01044             *(short int *)(&buffer[2+2*i]) = (short int)(angles[i] * 0x7FFF / MathUtils::Pi);
01045         if( toBird == -1 || toBird == number )
01046         {
01047             write( &buffer[1], 7 );
01048         }
01049         else 
01050         {
01051             buffer[0] = 0xF0+toBird;
01052             write( buffer, 8 );
01053         }
01054     }
01055 
01056     inline void Bird::setReferenceFrame( const float * angles, const int toBird )
01057     {
01058         char buffer[8];
01059         buffer[1] = 0x72;
01060         
01061         for( int i = 0; i < 3; i++ )
01062             *(short int *)(&buffer[2+2*i]) = (short int)(angles[i] * 0x7FFF / MathUtils::Pi);
01063         if( toBird == -1 || toBird == number )
01064         {
01065             write( &buffer[1], 7 );
01066         }
01067         else 
01068         {
01069             buffer[0] = 0xF0+toBird;
01070             write( buffer, 8 );
01071         }
01072     }
01073 
01074     inline int Bird::sendReset()
01075     {
01076         char buffer[1] = { 0x2F };
01077         write( buffer, 1 );
01078         OSUtils::sleep( 300 );
01079         return getErrorCode();
01080     }
01081 
01082 } // namespace ot
01083 
01084 
01085 #endif // OT_NO_FOB_SUPPORT
01086 
01087 /* 
01088  * ------------------------------------------------------------
01089  *   End of FOBModule.cxx
01090  * ------------------------------------------------------------
01091  *   Automatic Emacs configuration follows.
01092  *   Local Variables:
01093  *   mode:c++
01094  *   c-basic-offset: 4
01095  *   eval: (c-set-offset 'substatement-open 0)
01096  *   eval: (c-set-offset 'case-label '+)
01097  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
01098  *   eval: (setq indent-tabs-mode nil)
01099  *   End:
01100  * ------------------------------------------------------------ 
01101  */

copyright (c) 2006 Graz University of Technology