aflock.cpp

FreeWRLduneInputDevice和FreeWRL一起可以让用户用带有6DoF的输入设备检索3D VRML/X3D数据。它基于FreeWRL的"/tmp/inpdev"扩展传感器输入接口和w

/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 * -----------------------------------------------------------------
 * File:          aFlock.cpp,v
 * Date modified: 2001/05/22 14:54:46
 * Version:       1.41.6.2
 * -----------------------------------------------------------------
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

// Author:
//	Kevin Meinert
//      modified for white_dune by J. "MUFTI" Scheurich 
//      using things from HeadTracker/I3Stick API by J. Dean Brederson
//      using things from stty.c by David MacKenzie <[e-mail]> 
//
//      the streaming mode of the orginal VRjuggler Ascention Flock of birds 
//      tracking class seams to be not a good idea for white_dune...
// 
// Last Modified: Wed May 29 2002

#include <config.h>
#ifdef HAVE_AFLOCK

#include <string.h>
		
#include <fcntl.h>         // for open
#include <termios.h>
#include <unistd.h>        // for sleep, and ioctl
#include <stdlib.h>        // for system (to be deleted...)
#ifdef __linux__
# include <sys/ioctl.h>
//# include <asm/termbits.h>
//# include <sys/termios.h>
#endif
#include <sys/types.h>     // for open
#include <sys/stat.h>      // for open
#include <stdio.h>         // for fopen,fprintf
#include <assert.h>        // for assert

#if defined(__sun__) || defined(__hpux)
#include <sys/file.h>
#endif

#include "Aflock.h"

#ifdef HAVE_AFLOCK_DEBUG
#define AFLOCK_ERROR_CHECK(port,bird,what) 
//#define AFLOCK_ERROR_CHECK(port,bird,what) aflock_error_check(port,what,bird)

static void aflock_error_check(int port,int bird,char* what)
{ 
    char geterror[3];
    geterror[0]=bird+0xF0;
    geterror[1]='O';
    geterror[2]=0x0A;
    if (write(port, geterror, 3) == -1)
        fprintf(stderr, "Error sending geterror command while %s\n",what);
    tcdrain(port);
    char flockerror[1];
    if (read(port, flockerror, 1) == -1)
        fprintf(stderr, "Error getting errorcode command while %s\n",what);
    else if (flockerror[0]!=0)
        fprintf(stderr, "Errorcode: %d from bird %d while %s\n",
                        flockerror[0],bird,what);
}
#else
#define AFLOCK_ERROR_CHECK(port,bird,what)
#endif

const int Aflock::MAXCHARSTRINGSIZE = 256;
const int Aflock::mSleepFactor = 3;

//: Configure Constructor
// Give:                                                 <BR>
//   port - such as "/dev/ttyd3"                         <BR>
//   baud - such as 38400, 19200, 9600, 14400, etc...    <BR>
//   sync - sync type.                                   <BR>
//   block - blocking                                    <BR>
//   numBrds - number of birds in flock,                 <BR>
//   transmit - transmitter unit number,                 <BR>
//   hemi - hemisphere to track from,                    <BR>
//   filt - filtering type,                              <BR>
//   report -                                            <BR>
//   calfile - a calibration file, if "", then use none. <BR>
//                                                       <BR>
// Result: configures internal data members,
//         doesn't actually talk to the FOB yet.
Aflock::Aflock(const char* const port,
		const int& baud,
		const int& sync,
		const int& block,
		const int& numBrds,
		const int& transmit,
		const BIRD_HEMI& hemi,
		const BIRD_FILT& filt,
		const bool& sudden_lock,
		const char& report,
	  	const char* const calfile) :
		  _reportRate(report),
		  _hemisphere(hemi),
		  _filter(filt),
                  _sudden_output_change_lock(sudden_lock),
		  _portId(-1),
		  _baud(baud),
		  _syncStyle(sync),
		  _blocking(block),
		  _numBirds(numBrds),
		  _xmitterUnitNumber(transmit),
		  _usingCorrectionTable(false),
		  _active(false)
{
  if (port != NULL)
  	strncpy( _port, port, Aflock::MAXCHARSTRINGSIZE );

  // fix the report rate if it makes no sense.
  if ((_reportRate != 'Q') && (_reportRate != 'R') &&
      (_reportRate != 'S') && (_reportRate != 'T'))
  {
     // illegal report rate, defaulting to "every other cycle" (R)
     assert(false);
     _reportRate = 'R';
  }

  if (calfile != NULL && calfile[0] != '\0')
  {
  	strncpy( _calibrationFileName, calfile, Aflock::MAXCHARSTRINGSIZE );
  	this->initCorrectionTable(_calibrationFileName);
  	_usingCorrectionTable = true;
  }	
  _samplePrepared=false;
  _masterUseReceiver=false;
}

//: Destructor
Aflock::~Aflock()
{
    this->stop();
}

//: deliver number of active devices  

int Aflock::getNumberDevices(void) const
{
    if (_masterUseReceiver)
       return getNumBirds();
    else 
       return getNumBirds()+1;
}

bool Aflock::deliverData(int birdaddr)
{
    if (birdaddr==_xmitterUnitNumber)
       if (_masterUseReceiver)
          return(false);
    return(true);
}

//: see if the flock is active or not
const bool& Aflock::isActive() const
{
    return _active;
}

//: set the port to use
//  this will be a string in the form of the native OS descriptor <BR>
//  ex: unix - "/dev/ttyd3", win32 - "COM3" <BR>
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setPort(const char* const serialPort)
{
    if (_active)
    {
	fprintf(stderr,"Aflock: Cannot change the serial Port while active\n");
	return;
    }
    if (serialPort != NULL)
  	strncpy( _port, serialPort, Aflock::MAXCHARSTRINGSIZE );
}

//: get the port used
//  this will be a string in the form of the native OS descriptor <BR>
//  ex: unix - "/dev/ttyd3", win32 - "COM3"
const char* Aflock::getPort() const
{
    return _port;
}

//: set the baud rate
//  this is generally 38400, consult flock manual for other rates. <BR>
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setBaudRate(const int& baud)
{
    if (_active)
    {
	AFLOCK_PRINT("Aflock: Cannot change the baud rate while active\n");
	return;
    } else {
	_baud = baud;
    }
}

//: call this to connect to the flock device.
//  NOTE: flock.isActive() must be false to use this function
int Aflock::start()
{
    if (!_active)
    {
      AFLOCK_PRINT("Aflock: Getting ready....\n\n");

      AFLOCK_PRINT("Aflock: Opening port\n");
      Aflock::open_port( _port, _baud, _portId );
      if (_portId == -1)
      {
         fprintf(stderr,"unable to open port\n");
	 return 0;
      }

      // number of bytes per flock in groupmode:
#ifdef USE_AFLOCK_QUATERNION
      // 14 Byte for position/quaternion
      // 1 Byte for what flock
      #define NUMBER_OF_BYTES_PER_FLOCK 15
#else
      // 12 Byte for position/angles
      // 1 Byte for what flock
      #define NUMBER_OF_BYTES_PER_FLOCK 13
#endif
      _grouplength=_numBirds*NUMBER_OF_BYTES_PER_FLOCK;
      _groupbuffer=new char[_grouplength];

      AFLOCK_PRINT("Aflock: throw away dirt from port\n");
      Aflock::destroyDirt();

      if (_blocking==1)
         AFLOCK_PRINT("Aflock: Setting blocking\n");
      else if (_blocking==0)
         AFLOCK_PRINT("Aflock: Setting non blocking\n");
      Aflock::set_blocking( _portId, _blocking );

      if (_syncStyle!=0)
         AFLOCK_PRINT("Aflock: Setting sync\n");
      Aflock::set_sync( _portId, _syncStyle );


// Transmitter is at adress 1 (default)
      AFLOCK_PRINT("Aflock: Setting transmitter\n");
      Aflock::set_transmitter( _portId, _xmitterUnitNumber );

#ifdef USE_AFLOCK_QUATERNION
      AFLOCK_PRINT("Aflock: Setting pos_quaternion\n");
      Aflock::set_pos_quat( _portId, _xmitterUnitNumber );
#else
      AFLOCK_PRINT("Aflock: Setting pos_angles\n");
      Aflock::set_pos_angles( _portId, _xmitterUnitNumber );
#endif

      AFLOCK_PRINT("Aflock: Setting hemisphere\n");
      Aflock::set_hemisphere( _portId, _hemisphere, _xmitterUnitNumber );

      AFLOCK_PRINT("Aflock: Setting autoconfig\n");
      Aflock::set_autoconfig( _portId );

      if (_filter!=FILTER_DEFAULT) 
      {
         AFLOCK_PRINT("Aflock: Setting filter\n");
         Aflock::set_filter( _portId, _filter );
      }
      
      if (_sudden_output_change_lock)
         AFLOCK_PRINT("Aflock: lock sudden output change\n");
      else
         AFLOCK_PRINT("Aflock: unlock sudden output change\n");  
      Aflock::set_sudden_output_change_lock(_portId, 
                                            _sudden_output_change_lock);

      AFLOCK_PRINT("Aflock: Setting group\n");
      Aflock::set_group( _portId );

/// Currently not used (we are using point mode)
//      AFLOCK_PRINT("Aflock: Setting pickBird\n");
//      Aflock::pickBird( _xmitterUnitNumber,_portId );
//      AFLOCK_PRINT("Aflock: Setting rep_and_stream\n");
//      Aflock::set_rep_and_stream( _portId, _reportRate );

      AFLOCK_PRINT("Aflock: ready to go..\n");

      // flock is active.
      _active = true;

      // return success
      return 1;
    } 
    
    else
       return 0; // already sampling
}

// call this repeatedly to request data from the birds
// get the data later with flock.sample()
// NOTE: flock.isActive() must be true to use this function
int Aflock::prepareSample()
{
     // can't ask for data when not active.
     assert( _active == true );

     if (_samplePrepared)
        return 1;

     // ask for data from master (1) 
     char getpoint[2];
     getpoint[0]=0xF0+1;
     getpoint[1]='B';
     if (write(_portId, getpoint, 2) == -1) {
        fprintf(stderr, "Error sending polling command while reading 6DOF\n");
        tcdrain(_portId);
        return 0;
     }
     tcdrain(_portId);
     _samplePrepared=true;
     return 1;
}


//: call this repeatedly to update the data from the birds.
//  NOTE: flock.isActive() must be true to use this function
int Aflock::sample()
{
     // can't sample when not active.
     assert( _active == true );

     if (_samplePrepared==false)
        if (prepareSample()!=0)
           return 0;

     /* get a data record from the Flock */
     int numbytes;
     int errorcounter=0;
     int i=0;
     do {
        numbytes=read(_portId, _groupbuffer, _grouplength);
        if (numbytes<=0)
           {
           usleep(10);
           if (++errorcounter>MAX_READ_ERRORS)
              {
#ifdef HAVE_AFLOCK_DEBUG
              fprintf(stderr,"Aflock: cant get the bytes from getpoint command:");
              fprintf(stderr,"Aflock: got %d bytes of %d\n",i,_grouplength);
#endif
              _samplePrepared=false;
              destroyDirt();
              return 0;
              }
           if (numbytes<0)
              numbytes=0;
           }
        } while ((i+=numbytes)<_grouplength);

     _samplePrepared=false;

     int j=0;
     // for [1..n] birds, get their reading:
     for (int i=1; i <= getNumberDevices() && i <= MAX_SENSORS; i++)
	{
        // do not read values from _groupbuffer for Transmitter
        if (!deliverData(i))
	   continue;

	// you can never go above the maximum number of sensors.
	assert( j <= MAX_SENSORS );

        int begin=NUMBER_OF_BYTES_PER_FLOCK*j;
        char* buff=_groupbuffer;
        int flock=buff[begin+NUMBER_OF_BYTES_PER_FLOCK-1];

        if (flock > MAX_SENSORS)
           {
           fprintf(stderr,"Aflock: Invalid number of flock data in group: %d\n",
                   flock);
           return 0;
           }

        // Position
        xPos(flock) = rawToFloat(buff[1+begin],buff[0+begin])   * POSITION_RANGE;
        yPos(flock) = rawToFloat(buff[3+begin],buff[2+begin])   * POSITION_RANGE;
        zPos(flock) = rawToFloat(buff[5+begin],buff[4+begin])   * POSITION_RANGE;

        // Orientation
#ifdef USE_AFLOCK_QUATERNION
// check
        wQuat(flock) = rawToQuat(buff[7+begin],buff[6+begin]);
        xQuat(flock) = rawToQuat(buff[9+begin],buff[8+begin]);
        yQuat(flock) = rawToQuat(buff[11+begin],buff[10+begin]);
        zQuat(flock) = rawToQuat(buff[12+begin],buff[13+begin]);
#else  
        zRot(flock) = rawToFloat(buff[7+begin],buff[6+begin])   * ANGLE_RANGE;
        yRot(flock) = rawToFloat(buff[9+begin],buff[8+begin])   * ANGLE_RANGE;
        xRot(flock) = rawToFloat(buff[11+begin],buff[10+begin]) * ANGLE_RANGE;
#endif