aflock.cpp

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

        if (_usingCorrectionTable)
	   this->positionCorrect( xPos(flock), yPos(flock), zPos(flock));
        j++;
        }

     return 1;
}

// error timeout to wait between drain and flush (in usleep units)
#define ERROR_TIMEOUT 1000

//// destroy remaining dirt on the port to recover from errors
void Aflock::destroyDirt()
{
     tcdrain(_portId);
     usleep(ERROR_TIMEOUT);
     tcdrain(_portId);
}


//: stop the flock
int Aflock::stop()
{
    char   bird_command[4];

    AFLOCK_PRINT("Aflock: Try stopping the flock...");

    bird_command[0] = 'B';
    write( _portId, bird_command, 1 );

    usleep( 500 * Aflock::mSleepFactor );
    bird_command[0] = 'G';
    write( _portId, bird_command, 1 );

    usleep( 200 * Aflock::mSleepFactor );

    tcdrain(_portId);
    tcflush(_portId, TCIOFLUSH);
    tcdrain(_portId);

    close( _portId );
    _portId = -1;

    // flock is not active now.
    _active = false;

    AFLOCK_PRINT(" STOPPED \n");

    return 1;
}

//: Set the hemisphere that the transmitter transmits from.
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setHemisphere( const BIRD_HEMI& h )
{
    if (_active)
    {
	fprintf(stderr,"Aflock: Cannot change the hemisphere\n");
	return;
    } else {
	// Set it.
        _hemisphere = h;
    }
}

//: Set the type of filtering that the flock uses
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setFilterType( const BIRD_FILT& f )
{
    if (_active)
    {
	fprintf(stderr,"Aflock: Cannot change filter type while active\n");
	return;
    } else {
	// Set it.
	_filter = f;
    }
}

//: Set if sudden output changes of a bird should be locked
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setSuddenOutputChangeLock( const bool& b )
{
    if (_active)
    {
	fprintf(stderr,"Aflock: Cannot change sudden lock while active\n");
	return;
    } else {
	// Set it.
	_sudden_output_change_lock = b;
    }
}

//: Set the report rate that the flock uses
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setReportRate( const char& rRate )
{
    if (_active)
    {
	fprintf(stderr,"Aflock: Cannot change report rate while active\n");
	return;
    } else {
	// Set it.
	_reportRate = rRate;
    }
}

//: Set the unit number of the transmitter
//  give - an integer that is the same as the dip switch
//         setting on the transmitter box (for the unit number) <BR>
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setTransmitter( const int& Transmit )
{
  if (_active)
  {
      fprintf(stderr,"Aflock: Cannot change transmitter while active\n");
      return;
  } else {
      // Set it.
      _xmitterUnitNumber = Transmit;
  }
}

//: Set the number of birds to use in the flock.
//  give - an integer number not more than the number of
//         birds attached to the system <BR>
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setNumBirds( const int& n )
{
    if (_active)
    {
	fprintf(stderr,"Aflock: Cannot change num birds while active\n");
	return;
    } else {
	// Set it.
	_numBirds = n;
    }
}

//: set the video sync type
//  this option allows the Flock to syncronize its pulses with
//  your video display.  This will eliminate most flicker caused
//  by the magnetic distortion. <BR>
//  - Refer to your flock manual for what number to use.
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setSync(const int& sync)
{
  if (_active)
  {
      fprintf(stderr,"Aflock: Cannot change report rate while active\n");
      return;
  } else {
      // Set it.
      _syncStyle = sync;
  }
}

//: set blocking of flock
//  see flock manual for details.
//  NOTE: flock.isActive() must be false to use this function
void Aflock::setBlocking( const int& blVal )
{
  if (_active)
  {
      fprintf(stderr,"Aflock: Cannot change blocking while active\n");
      return;
  } else {
      // Set it.
      _blocking = blVal;
  }
}

//: with the calibration table info, correct a given position
//  give - a position in x, y, z euclidian coordinates
//  returns - a corrected position in x, y, z euclidian coordinates
void Aflock::positionCorrect( float& x, float& y, float& z )
{
    int xlo,ylo,zlo,xhi,yhi,zhi;
    float a,b,c,a1,b1,c1;

    // Find corners
    xlo = (int)(x-caltable.xmin);
    ylo = (int)(y-caltable.ymin);
    zlo = (int)(z-caltable.zmin);
    xhi = xlo+1;
    yhi = ylo+1;
    zhi = zlo+1;
    a1 = 1 - (a = caltable.xmin + xhi - x);
    b1 = 1 - (b = caltable.ymin + yhi - y);
    c1 = 1 - (c = caltable.zmin + zhi - z);

    x = a * b* c*caltable.xloc[xlo][ylo][zlo] +
        a * b*c1*caltable.xloc[xlo][ylo][zhi] +
        a *b1* c*caltable.xloc[xlo][yhi][zlo]+
        a *b1*c1*caltable.xloc[xlo][yhi][zhi]+
        a1* b* c*caltable.xloc[xhi][ylo][zlo]+
        a1* b*c1*caltable.xloc[xhi][ylo][zhi]+
        a1*b1* c*caltable.xloc[xhi][yhi][zlo]+
        a1*b1*c1*caltable.xloc[xhi][yhi][zhi];
    y = a * b* c*caltable.yloc[xlo][ylo][zlo] +
        a * b*c1*caltable.yloc[xlo][ylo][zhi] +
        a *b1* c*caltable.yloc[xlo][yhi][zlo]+
        a *b1*c1*caltable.yloc[xlo][yhi][zhi]+
        a1* b* c*caltable.yloc[xhi][ylo][zlo]+
        a1* b*c1*caltable.yloc[xhi][ylo][zhi]+
        a1*b1* c*caltable.yloc[xhi][yhi][zlo]+
        a1*b1*c1*caltable.yloc[xhi][yhi][zhi];
    z = a * b* c*caltable.zloc[xlo][ylo][zlo] +
        a * b*c1*caltable.zloc[xlo][ylo][zhi] +
        a *b1* c*caltable.zloc[xlo][yhi][zlo]+
        a *b1*c1*caltable.zloc[xlo][yhi][zhi]+
        a1* b* c*caltable.zloc[xhi][ylo][zlo]+
        a1* b*c1*caltable.zloc[xhi][ylo][zhi]+
        a1*b1* c*caltable.zloc[xhi][yhi][zlo]+
        a1*b1*c1*caltable.zloc[xhi][yhi][zhi];
    return;
}

/// Warning: initCorrectionTable is untested

//: init the correction table from a file
//  give - a file name of the calibration file.
//  result - initializes the correction table with the file's info
void Aflock::initCorrectionTable( const char* const fName )
{
  int i,j,k, xsize,ysize,zsize;
  float dump;
  FILE* inFile;

  AFLOCK_PRINT("	  Initializing calibration table ... ");
  AFLOCK_PRINT(fName);
  inFile=fopen(fName,"r");
  if (!inFile)
  {
	fprintf(stderr,"Unable to open calibration.table\n");
        return;
  } else {
     AFLOCK_PRINT( "   Calibration table opened sucessfully.");
  }

  fscanf(inFile,"%f %f %f %f %f %f",
                &caltable.xmin,&caltable.xmax,
                &caltable.ymin,&caltable.ymax,
                &caltable.zmin,&caltable.zmax);

  xsize = (int) (caltable.xmax - caltable.xmin) + 1;
  ysize = (int) (caltable.ymax - caltable.ymin) + 1;
  zsize = (int) (caltable.zmax - caltable.zmin) + 1;

  for (i = 0; i < xsize; i++)
    for (j = 0; j < ysize; j++)
      for (k = 0; k < zsize; k++)
         {
            fscanf(inFile,"%f %f %f %f %f %f",
	           &dump , &dump , &dump,
		   &caltable.xloc[i][j][k],
		   &caltable.yloc[i][j][k],
		   &caltable.zloc[i][j][k]);
	 }

   fclose(inFile);
}

 float& Aflock::xPos( const int& i )  { assert( i < MAX_SENSORS ); return _position[i][0]; }
 float& Aflock::yPos( const int& i )  { assert( i < MAX_SENSORS ); return _position[i][1]; }
 float& Aflock::zPos( const int& i )  { assert( i < MAX_SENSORS ); return _position[i][2]; }
#ifdef USE_AFLOCK_QUATERNION
 float& Aflock::xQuat( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][0]; }
 float& Aflock::yQuat( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][1]; }
 float& Aflock::zQuat( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][2]; }
 float& Aflock::wQuat( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][3]; }
#else
 float& Aflock::zRot( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][0]; }
 float& Aflock::yRot( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][1]; }
 float& Aflock::xRot( const int& i )  { assert( i < MAX_SENSORS ); return _orientation[i][2]; }
#endif

///////////////////////////////////////////////////////////////////
// Local functions to Aflock.cpp
//////////////////////////////////////////////////////////////////

float Aflock::rawToFloat( char& r1, char& r2 )
{
    // I3Stick has something like
    // ((float)((short)((buff[1] & 0x7F) << 9) |
    //          (short)((buff[0] & 0x7F) << 2)) / 0x7FFF);

   short int ival1,ival2,val;
   ival1 = r1 & 0x7f;
   ival2 = r2 & 0x7f;
   val = (ival1 << 9) | ival2<<2;
   return ((float)val) / 0x7fff;
}

float Aflock::rawToQuat(char& r1, char& r2)
{
     signed short int val;
     unsigned short int ival1, ival2; 
     ival1 = r1 ;
     ival2 = r2 ;
     val = (ival1 << 8) | ival2;
     float ret= ((float)val) / (float)0x7fff ;
printf("ret %f\n",ret);
     return ret;
}

//: RS232 To FBB Command 
//  adress next RS232 command to bird number "birdID"
void  Aflock::pickBird( const int& birdID, const int& port )
{
    char buff = 0xF0 + birdID;
    write( port, &buff, 1 );

    usleep ( 100 * Aflock::mSleepFactor );
    tcdrain(_portId);

    AFLOCK_ERROR_CHECK(port,"RS232 command",birdID);
}

//: Open the port.
//  give - a serial port
//  give - a baud rate
//  returns portId twice (created by the open function)
//  NOTE: portId is returned from both ends of this function.
//  if portId == -1 then function failed to open the port.
int Aflock::open_port( const char* const serialPort,
			const int& baud,
			int& portId )
{
    ///////////////////////////////////////////////////////////////////
    // Open and close the port to reset the tracker, then
    // Open the port
    ///////////////////////////////////////////////////////////////////
    portId = open( serialPort, O_RDWR | O_NDELAY);
    if (portId == -1)
    {
	fprintf(stderr,"  port reset failed (because port open failed)\n");
	return portId;
    } else {
	sleep(2);
	close( portId );
	AFLOCK_PRINT( "  port reset successfully (port opened then closed)\n");
    }

    portId = open( serialPort, O_RDWR | O_NDELAY);

    if (portId == -1)
    {
	fprintf(stderr,"  port open failed\n");
	return portId;
    } else {
	AFLOCK_PRINT( "  port open successfully\n");
    }

    //////////////////////////////////////////////////////////////////
    // Setup the port, current setting is 38400 baud
    //
    //////////////////////////////////////////////////////////////////

    AFLOCK_PRINT( "  Getting current term setting\n");
    termios termIoPort;
    /* Initialize to all zeroes so there is no risk memcmp will report a
       spurious difference in an uninitialized portion of the structure.  */
    memset (&termIoPort, 0, sizeof(termIoPort));
    if (tcgetattr(portId, &termIoPort))
       perror(serialPort);
     
    // Set the flags to 0 to clear out any cruft and to ensure that we are
    // setting fresh values.
    termIoPort.c_iflag = termIoPort.c_oflag = termIoPort.c_lflag = 0;

#ifndef __sgi
    termIoPort.c_lflag = N_TTY;
#endif

    // get the B* format baud rate
    // i.e.: B38400 is baud = 38400
    int magicBaudRate = 0;
    switch (baud)
    {
	case 150: magicBaudRate = B150; break;
	case 200: magicBaudRate = B200; break;
	case 300: magicBaudRate = B300; break;
	case 600: magicBaudRate = B600; break;
	case 1200: magicBaudRate = B1200; break;
	case 1800: magicBaudRate = B1800; break;
	case 2400: magicBaudRate = B2400; break;
	case 4800: magicBaudRate = B4800; break;
	case 9600: magicBaudRate = B9600; break;
	case 19200: magicBaudRate = B19200; break;
	
#ifndef _POSIX_SOURCE
	case 57600: magicBaudRate = B57600; break;
#   ifdef B76800
	    case 76800: magicBaudRate = B76800; break;
#   endif
	case 115200: magicBaudRate = B115200; break;
#endif

        case 38400:
        default:
          magicBaudRate = B38400;
          break;
    }

    termIoPort.c_cflag = CS8 | CLOCAL | CREAD;
//    termIoPort.c_iflag = IGNBRK;
    termIoPort.c_cc[0] = 0;			/* no delete/interrupt */
    termIoPort.c_cc[VMIN]  = 0;			/* character or timeout 
                                                   satisfies read */
    termIoPort.c_cc[VTIME] = 1;			/* 100 millisecond timeout */

#ifdef __linux__
    cfsetispeed (&termIoPort, magicBaudRate);
    cfsetospeed (&termIoPort, magicBaudRate);

    struct termios new_mode;

    if (tcsetattr (portId, TCSADRAIN, &termIoPort))
       perror(serialPort);