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 errorsvoid Aflock::destroyDirt(){     tcdrain(_portId);     usleep(ERROR_TIMEOUT);     tcdrain(_portId);}//: stop the flockint 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 functionvoid 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 functionvoid 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 functionvoid 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 functionvoid 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 functionvoid 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 functionvoid 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 functionvoid 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 functionvoid 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 coordinatesvoid 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 infovoid 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);