/*************** <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 <djm@gnu.ai.mit.edu> ////      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)#endifconst 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;}//: DestructorAflock::~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 notconst 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 functionvoid 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 functionvoid 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 functionint 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 functionint 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 functionint 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