Flock of Birds Library: fob.h Source File

00001 /*
00002 -------------------------------------------------------------------------------
00003 libfob - C++ interface to Ascension Technology Corporation's
00004          Flock of Birds position and orientation measurement system.
00005 Copyright (C) 2002 Nathan Cournia
00006 
00007 This library is free software; you can redistribute it and/or
00008 modify it under the terms of the GNU Lesser General Public
00009 License as published by the Free Software Foundation; either
00010 version 2.1 of the License, or (at your option) any later version.
00011 
00012 This library is distributed in the hope that it will be useful,
00013 but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015 Lesser General Public License for more details.
00016 
00017 You should have received a copy of the GNU Lesser General Public
00018 License along with this library; if not, write to the Free Software
00019 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020 -------------------------------------------------------------------------------
00021 */
00022 
00023 //FILE:         fob.h
00024 //AUTHORS:      Nathan Cournia <nathan@cournia.com>
00025 //              Eric Medlin <emedlin@vr.clemson.edu>
00026 
00027 #ifndef COURNIA_FOB_H
00028 #define COURNIA_FOB_H 1
00029 
00030 #include <cstring>
00031 #include <vector>
00032 #include <string>
00033 #include <cstdarg>
00034 #include <pthread.h>
00035 #include <termios.h>
00036 #include <unistd.h>
00037 #include "fob/quaternion.h"
00038 #include "fob/vector.h"
00039 
00040 #define _POSIX_SOURCE 1
00041 
00042 //assume normal addressing mode (code actually checks for it)
00043 //normal address mode only supports 14 birds
00044 const int FOB_MAX_BIRDS = 14;
00045 
00046 extern "C" {
00048 
00053     void* get_flock_data( void *flock );
00054 }
00055 
00057 
00115 class fob
00116 {
00117 public:
00118     typedef unsigned char mode; 
00119     static const mode POSITION; 
00120     static const mode ORIENTATION; 
00121     static const mode BUTTONS; 
00122 
00123     static const unsigned char BUTTON_NONE; 
00124     static const unsigned char BUTTON_LEFT; 
00125     static const unsigned char BUTTON_MIDDLE; 
00126     static const unsigned char BUTTON_RIGHT; 
00127     
00129     class bird {
00130      private:
00131         fob& m_flock; 
00132         mutable pthread_mutex_t m_data_mutex; 
00133         math::vector3 m_position; 
00134         math::vector3 m_fob_angles; 
00135         math::vector3 m_angles; 
00136         math::quaternion m_quaternion; 
00137         math::matrix4 m_matrix; 
00138         math::quaternion m_rotation; 
00139         unsigned int m_address; 
00140         bool m_sensor; 
00141         bool m_transmitter; 
00142         mode m_mode; 
00143         unsigned char m_buttons; 
00144         bool m_ori_dirty; 
00145 
00146         friend class fob;
00147 
00149         inline void lock_data( void ) const {
00150             pthread_mutex_lock( &m_data_mutex );
00151         }
00152 
00154         inline void unlock_data( void ) const {
00155             pthread_mutex_unlock( &m_data_mutex );
00156         }
00157 
00159         bool unpack_pos_angle( unsigned char *buffer, int size );
00160         
00162         bool unpack_angle( unsigned char *buffer, int size );
00163 
00165         bool unpack_pos( unsigned char *buffer, int size );
00166 
00168         bool update( unsigned char *buffer, int size );
00169 
00171         void update_orientation( void );
00172 
00173      public:
00175         bird( fob& parent ): m_flock( parent ),
00176             m_address( 0 ), m_sensor( false ), 
00177             m_transmitter( false ), m_mode( 0x0 ), m_buttons( 0x0 ),
00178             m_ori_dirty( false )
00179         {
00180             pthread_mutex_init( &m_data_mutex, NULL );
00181         }
00182 
00184         bird( const bird& cpy ): m_flock( cpy.m_flock ) {
00185             //initialize mutex
00186             pthread_mutex_init( &m_data_mutex, NULL );
00187             
00188             //lock data 
00189             cpy.lock_data( );
00190 
00191             //copy data
00192             m_position = cpy.m_position;
00193             m_fob_angles = cpy.m_fob_angles;
00194             m_angles = cpy.m_angles;
00195             m_quaternion = cpy.m_quaternion;
00196             m_matrix = cpy.m_matrix;
00197             m_address = cpy.m_address;
00198             m_sensor = cpy.m_sensor;
00199             m_transmitter = cpy.m_transmitter;
00200             m_mode = cpy.m_mode;
00201             m_buttons = cpy.m_buttons;
00202             m_ori_dirty = cpy.m_ori_dirty;
00203 
00204             //unlock data
00205             cpy.unlock_data( );
00206         }
00207 
00209         ~bird( void ) {
00210             pthread_mutex_destroy( &m_data_mutex );
00211         }
00212 
00214         inline const bird& operator= ( const bird& rhs ) {
00215             //account for self assignment (which would cause a deadlock)
00216             if( &rhs == this ) return *this;
00217 
00218             //lock data (FIXME possible deadlock here)
00219             lock_data( );
00220             rhs.lock_data( );
00221 
00222             //copy data
00223             m_flock = rhs.m_flock;
00224             m_position = rhs.m_position;
00225             m_fob_angles = rhs.m_fob_angles;
00226             m_angles = rhs.m_angles;
00227             m_quaternion = rhs.m_quaternion;
00228             m_matrix = rhs.m_matrix;
00229             m_address = rhs.m_address;
00230             m_sensor = rhs.m_sensor;
00231             m_transmitter = rhs.m_transmitter;
00232             m_mode = rhs.m_mode;
00233             m_buttons = rhs.m_buttons;
00234             m_ori_dirty = rhs.m_ori_dirty;
00235 
00236             //unlock data
00237             rhs.unlock_data( );
00238             unlock_data( );
00239 
00240             //return reference to self
00241             return *this;
00242         }
00243 
00245         inline void get_position( math::vector3& output ) const {
00246             lock_data( );
00247             if( m_ori_dirty ) {
00248                 output = m_position;
00249             }
00250             unlock_data( );
00251         }
00252 
00254         inline void get_position( real_t output[ 3 ] ) const {
00255             lock_data( );
00256             std::memcpy( output, static_cast<const real_t*>( m_position ), sizeof( real_t ) * 3 );
00257             unlock_data( );
00258         }
00259 
00261 
00264         inline void get_angles( math::vector3& output ) {
00265             lock_data( );
00266             if( m_ori_dirty ) {
00267                 update_orientation( );
00268             }
00269             output = m_angles;
00270             unlock_data( );
00271         }
00272 
00274 
00277         inline void get_angles( real_t output[ 3 ] ) {
00278             lock_data( );
00279             if( m_ori_dirty ) {
00280                 update_orientation( );
00281             }
00282             std::memcpy( output, static_cast<const real_t*>( m_angles ), sizeof( real_t ) * 3 );
00283             unlock_data( );
00284         }
00285 
00287         inline void get_quaternion( math::quaternion& output ) {
00288             lock_data( );
00289             if( m_ori_dirty ) {
00290                 update_orientation( );
00291             }
00292             output = m_quaternion;
00293             unlock_data( );
00294         }
00295 
00297         /*
00298          * Returns the quaternion in x y z w order.
00299          */
00300         inline void get_quaternion( real_t output[ 4 ] ) {
00301             lock_data( );
00302             if( m_ori_dirty ) {
00303                 update_orientation( );
00304             }
00305             std::memcpy( output, static_cast<const real_t*>( m_quaternion.vec( ) ), sizeof( real_t ) * 3 );
00306             output[ 3 ] = m_quaternion.w( );
00307             unlock_data( );
00308         }
00309         
00311 
00314         inline void get_matrix( math::matrix4& output ) {
00315             lock_data( );
00316             if( m_ori_dirty ) {
00317                 update_orientation( );
00318             }
00319             output = m_matrix;
00320             unlock_data( );
00321         }
00322 
00324 
00327         inline void get_matrix( real_t output[ 16 ] ) {
00328             lock_data( );
00329             if( m_ori_dirty ) {
00330                 update_orientation( );
00331             }
00332             std::memcpy( output, static_cast<const real_t*>( m_matrix ), sizeof( real_t ) * 16 );
00333             unlock_data( );
00334         }
00335         
00337 
00341         bool set_mode( fob::mode mask );
00342 
00344         void set_rotation( const math::quaternion& rot );
00345 
00347 
00359         inline unsigned char get_buttons( void ) const {
00360             return m_buttons;
00361         }
00362     };
00363 
00365     typedef std::vector<fob::bird*> bird_list;
00366     
00368     enum hemisphere {
00369         AFT,
00370         FORWARD,
00371         LEFT,
00372         RIGHT,
00373         UP,
00374         DOWN
00375     };
00376 
00378     enum port_speed {
00379         FAST,
00380         SLOW
00381     };
00382 
00384     enum error_level {
00385         WARNING1,
00386         WARNING2,
00387         FATAL
00388     };
00389 
00391     struct error_t {
00392         const char *msg;   
00393         error_level level; 
00394     };
00395 
00396  private:
00397     bool m_error; 
00398     std::string m_error_msg; 
00399 
00400     pthread_t m_flock_thread; 
00401     bird_list m_birds; 
00402     bool m_open; 
00403     int m_device; 
00404     struct termios m_save_tio; 
00405     unsigned int m_master; 
00406     bool m_fly; 
00407     bool m_group; 
00408     pthread_mutex_t m_mutex; 
00409     unsigned char m_save; 
00410     hemisphere m_hemisphere; 
00411     unsigned long m_sleep; 
00412     error_level m_min_error_level; 
00413     
00415     inline void clear_error( void ) {
00416         m_error = false;
00417         m_error_msg = "none";
00418     }
00419 
00421     inline void set_error( const char *fmt, ... ) {
00422         char buffer[ 1024 ];
00423         va_list ap;
00424         va_start( ap, fmt );
00425         vsnprintf( buffer, 1024, fmt, ap );
00426         va_end( ap );
00427         m_error = true;
00428         m_error_msg = buffer;
00429     }
00430 
00432     inline void lock( void ) {
00433         pthread_mutex_lock( &m_mutex );
00434     }
00435 
00437     inline void unlock( void ) {
00438         pthread_mutex_unlock( &m_mutex );
00439     }
00440 
00442     friend void* get_flock_data( void *flock );
00443 
00445     struct command {
00446         unsigned char cmd; 
00447         int option_bytes; 
00448     };
00449     
00451     struct examine_option {
00452         unsigned char param; 
00453         int reply_bytes; 
00454     };
00455 
00457     struct change_option {
00458         unsigned char param; 
00459         int option_bytes; 
00460     };
00461 
00463     void clear_device( void );
00464 
00466     int read( unsigned char *output, int bytes );
00467 
00469     bool send_cmd( unsigned char cmd );
00470 
00472 
00477     bool send_cmd( fob::command cmd, unsigned char *options );
00478 
00480 
00485     bool examine( examine_option param, unsigned char *output );
00486 
00488 
00493     bool change( change_option param, unsigned char *option );
00494 
00496 
00502     bool check_address_mode( void );
00503 
00505 
00508     bool load_flock_status( void );
00509 
00511 
00514     bool auto_configure( unsigned int num_birds );
00515 
00517 
00522     bool set_rts( bool high );
00523 
00525     bool set_hemisphere( fob::bird& b, fob::hemisphere hemi );
00526     
00528 
00532     bool reset( void );
00533 
00535 
00541     bool check_error( void );
00542 
00544     bool select_bird( bird& b );
00545 
00547     bool select_bird( int bird_addr );
00548 
00550 
00554     bool update( void );
00555 
00557     void cmd_sleep( void ) const {
00558         cmd_sleep( m_sleep );
00559     }
00560 
00562 
00565     void cmd_sleep( unsigned long us ) const {
00566         //if sleep time is 0, don't call sleep (since it may
00567         //give up the processor)
00568         if( us > 1 ) {
00569             usleep( us );
00570         }
00571     }
00572 
00573     //see fob.cpp for definitions
00574     static const unsigned char STREAM;
00575     static const unsigned char POINT;
00576     static const unsigned char SLEEP;
00577     static const unsigned char EXAMINE;
00578     static const unsigned char CHANGE;
00579     static const unsigned char MODE_POS;
00580     static const unsigned char MODE_ANG;
00581     static const unsigned char MODE_POS_ANG;
00582     static const unsigned char MODE_POS_MAT;
00583     static const unsigned char MODE_POS_QUAT;
00584     static const unsigned char MODE_QUAT;
00585     static const unsigned char FBB_RESET;
00586     static const unsigned char RUN;
00587     static const command HEMISPHERE;
00588     static const command BUTTON_MODE;
00589     static const command REF_FRAME_1;
00590     static const command REF_FRAME_2;
00591     static const examine_option BIRD_STATUS;
00592     static const examine_option ERROR_CODE;
00593     static const examine_option MODEL_ID;
00594     static const examine_option REF_FRAME_1_EXAMINE;
00595     static const examine_option ADDR_MODE;
00596     static const examine_option FLOCK_STATUS;
00597     static const change_option REF_FRAME_1_ENABLE;
00598     static const change_option GROUP;
00599     static const change_option FBB_AUTO_CONFIG;
00600     static const float SCALE;
00601     static const float ANGLE_SCALE;
00602     
00603 public:
00605 
00609     fob( void );
00610 
00612 
00620     fob( const std::string& device_name, fob::hemisphere hemi,
00621       port_speed speed = FAST, unsigned long sleep_ms = 500 );
00622 
00624 
00628     ~fob( void );
00629 
00631     /* \param device_name Serial port the flock is connected too.
00632      * \param hemi Hemisphere in which all birds of the flock will operate.
00633      * \param speed Speed at which the host system will communicate with the flock hardware.
00634      *              Use fob::SLOW for older flock hardware.
00635      * \param sleep_ms Amount of time to sleep between sending commands to the flock hardware (in ms).
00636      */
00637     const fob& open( const std::string& device_name, 
00638         fob::hemisphere hemisphere, port_speed speed = FAST,
00639         unsigned long sleep_ms = 500 );
00640 
00642     bool close( void );
00643 
00645     inline fob::bird_list& get_birds( void ) {
00646         return m_birds;
00647     }
00648 
00650     bool fly( void );
00651 
00653     bool print_bird_status( void );
00654 
00656     inline operator bool ( void ) const {
00657         return !m_error;
00658     }
00659 
00661     inline const std::string& get_error( void ) const {
00662         return m_error_msg;
00663     }
00664 
00666 
00675     inline void set_sleep( unsigned long ms ) {
00676         //convert from ms to us
00677         m_sleep = ms * 1000;
00678     }
00679 
00681     inline unsigned long get_sleep( void ) const {
00682         return m_sleep / 1000;
00683     }
00684 
00686 
00689     inline void set_error_level( error_level lvl ) {
00690         m_min_error_level = lvl;
00691     }
00692 };
00693 
00694 #endif