aflock.cpp

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

      /* POSIX (according to Zlotnick's book) tcsetattr returns zero if
	 it performs *any* of the requested operations.  This means it
	 can report `success' when it has actually failed to perform
	 some proper subset of the requested operations.  To detect
	 this partial failure, get the current terminal attributes and
	 compare them to the requested ones.  */

      /* Initialize to all zeroes so there is no risk memcmp will report a
	 spurious difference in an uninitialized portion of the structure.  */
      memset (&new_mode, 0, sizeof (new_mode));
      if (tcgetattr (portId, &new_mode))
         perror(serialPort);

      /* Normally, one shouldn't use memcmp to compare structures that
	 may have `holes' containing uninitialized data, but we have been
	 careful to initialize the storage of these two variables to all
	 zeroes.  One might think it more efficient simply to compare the
	 modified fields, but that would require enumerating those fields --
	 and not all systems have the same fields in this structure.  */

      if (memcmp (&termIoPort, &new_mode, sizeof (termIoPort)) != 0)
	{
#ifdef CIBAUD
	  /* SunOS 4.1.3 (at least) has the problem that after this sequence,
	     tcgetattr (&m1); tcsetattr (&m1); tcgetattr (&m2);
	     sometimes (m1 != m2).  The only difference is in the four bits
	     of the c_cflag field corresponding to the baud rate.  To save
	     Sun users a little confusion, don't report an error if this
	     happens.  But suppress the error only if we haven't tried to
	     set the baud rate explicitly -- otherwise we'd never give an
	     error for a true failure to set the baud rate.  */

	  new_mode.c_cflag &= (~CIBAUD);
	  if (memcmp (&termIoPort, &new_mode, sizeof (termIoPort)) != 0)
#endif
	      fprintf(stderr,
		     " unable to perform all requested operations %s",
		     serialPort);
#else
    /* set Ascension FOB port parameters */
    termIoPort.c_ospeed = magicBaudRate;	/* out */
    termIoPort.c_ispeed = magicBaudRate;	/* in : not supported 
                                                   (in = out) */
    if ((ioctl(portId, TCSETA, &termIoPort)) == -1) {
	fprintf(stderr, "Error setting attributes for serial port %s:\n", 
                serialPort);
#ifdef HAVE_OSERROR
	fprintf(stderr,"oserror %d\n",oserror());
#endif
        perror(serialPort);
#endif
    }
   char command[1024];

#ifdef USE_STTY_LINUX
// when termios do not work 8-(

   snprintf(command,1023,"stty 38400 cs8 cread clocal min 0 time 1 -F /dev/ttyS%c",
            serialPort[strlen(serialPort)-1]);
   system(command);
#endif
#ifdef HAVE_AFLOCK_DEBUG
   // only used for debugging 
   // run "stty -a 
#  ifdef __sgi
   snprintf(command,1023,"stty -a F /dev/ttyd%c",
            serialPort[strlen(serialPort)-1]);
#  else
   snprintf(command,1023,"stty -a F /dev/ttyS%c",
            serialPort[strlen(serialPort)-1]);
#  endif
   system(command);
#endif

    // return the portID
    return portId;
}

void Aflock::set_blocking( const int& port, const int& blocking )
{
    //////////////////////////////////////////////////////////////////
    // Setup a non/blocked port & Flush port
    //////////////////////////////////////////////////////////////////
    static int blockf,nonblock;
    int flags;

    flags = fcntl( port,F_GETFL,0 );

    // Turn blocking on
    blockf   = flags & ~FNDELAY;

    // Turn blocking off
    nonblock = flags | FNDELAY;

    // 0 Non Blocked
    // 1 Blocked
    fcntl( port, F_SETFL, blocking ? blockf : nonblock );

    usleep( 1000 * Aflock::mSleepFactor );
//    tcflush(port, TCIOFLUSH);
}

void Aflock::set_sync( const int& port, const int& sync )
{
    /////////////////////////////////////////////////////////////////
    // Set CRT sync: (manual page 82)
    //   set crt sync
    //   nosync    -   0
    //   > 72Hz    -   1 (type 1)
    //                 2 (type 2)
    /////////////////////////////////////////////////////////////////
    unsigned char buff[4] = {'A', 1};
    buff[1] = sync;
    write( port,buff,2 );
    tcdrain(port);
}


void Aflock::set_hemisphere( const int& port,
			const BIRD_HEMI& hem,
			const int& transmitter )
{
    /////////////////////////////////////////////////////////////////
    // Set Hemisphere for birds taking input
    //
    //  buff   [1]   [2]
    // Front: 0x00, 0x00
    // Aft  : 0x00, 0x01
    // Upper: 0x0C, 0x01
    // Lower: 0x0C, 0x00
    // Left : 0x06, 0x01
    // Right: 0x06, 0x00
    /////////////////////////////////////////////////////////////////
    char buff[3];
    buff[0] = 'L';
    for (int i = 1; i <= getNumberDevices(); i++)
    {
	if (!deliverData(i))  
	    continue;
	pickBird( i,port );
	switch (hem)
	{
	case FRONT_HEM:
	    buff[1] = 0x00;
	    buff[2] = 0x00;
	    break;
	case AFT_HEM:
	    buff[1] = 0x00;
	    buff[2] = 0x01;
	    break;
	case UPPER_HEM:
	    buff[1] = 0x0C;
	    buff[2] = 0x01;
	    break;
	case LOWER_HEM:
	    buff[1] = 0x0C;
	    buff[2] = 0x00;
	    break;		
	case LEFT_HEM:
	    buff[1] = 0x06;
	    buff[2] = 0x01;
	    break;
	case RIGHT_HEM:
	    buff[1] = 0x06;
	    buff[2] = 0x00;
	    break;
	}
	write( port, buff, 3 );

	usleep( 500 * Aflock::mSleepFactor );
        tcdrain(port);

    }
}

// Streaming mode currently not used
void Aflock::set_rep_and_stream(const int& port, const char& reportRate)
{
    char buff[1];
    /////////////////////////////////////////////////////////////////
    // Set report rate
    //             Q  Every cycle
    //  buff[0] - 'R' Every other bird cycle
    //             S  every 8 cycles
    //             T  every 32 cycles
    /////////////////////////////////////////////////////////////////
    buff[0] = reportRate;
    write( port, buff, 1 );

    usleep( 2000 * Aflock::mSleepFactor );
    tcdrain(port);

    ////////////////////////////////////////////////////////////////
    // set stream mode
    ////////////////////////////////////////////////////////////////
    buff[0] = '@';
    write( port, buff, 1 );

    usleep( 500 * Aflock::mSleepFactor );
    tcdrain(port);

}

void Aflock::set_pos_quat(const int& port, const int& transmitter)
{
    //////////////////////////////////////////////////////////////////
    // Set Position Quaternion
    /////////////////////////////////////////////////////////////////
    char buff[1];
    for (int i = 1; i <= getNumberDevices(); i++)
    {
	if (!deliverData(i))
		continue;
	Aflock::pickBird( i,port );

	buff[0] = ']';
	write( port, buff, 1 );

	usleep( 500 * Aflock::mSleepFactor );
        tcdrain(port);

      AFLOCK_ERROR_CHECK(port,"Position/Quaternion command",transmitter);
    }
}

void Aflock::set_pos_angles(const int& port, const int& transmitter)
{
    //////////////////////////////////////////////////////////////////
    // Set Position Angles
    /////////////////////////////////////////////////////////////////
    char buff[1];
    for (int i = 1; i <= getNumberDevices(); i++)
    {
	if (!deliverData(i))
		continue;
	Aflock::pickBird( i,port );

	buff[0] = 'Y';
	write( port, buff, 1 );

	usleep( 500 * Aflock::mSleepFactor );
        tcdrain(port);

      AFLOCK_ERROR_CHECK(port,"Position/Angles command",transmitter);
    }
}

void Aflock::set_filter(const int& port, const BIRD_FILT& filter)
{
    // a filter value of AC_NARROW | AC_WIDE | DC_FILTER 
    // turns all filters on
    ///////////////////////////////////////////////////////////////
    // Turn filters on (manual page 48)
    // 0s turn AC NARROW notch filter ON
    //         AC WIDE notch filter ON
    //         DC filter ON
    ///////////////////////////////////////////////////////////////
    for (int i = 1; i <= getNumberDevices(); i++)
    {
       if (!deliverData(i))
          continue;
       pickBird( i,port );
       char buff[4];
       buff[0] = 'P';
       buff[1] = (~filter) & 7;
       buff[2] = 0x00;
       buff[3] = 0;
       write(port, buff, 4);

       //TODO: Do I need to sleep here?
       usleep(12000 * Aflock::mSleepFactor);
       tcdrain(port);
    }
}

void Aflock::set_transmitter(const int& port, const int& transmitter)
{
    ///////////////////////////////////////////////////////////////
    // Sets up the device for Transmitting (manual page 67)
    // Command (0x30) for Next Transmitter
    ///////////////////////////////////////////////////////////////
    char buff[2];
    buff[0] = (unsigned char) (0x30);
    buff[1] = (unsigned char) transmitter  << 4;
    write(port, buff, 2);

    usleep(12000 * Aflock::mSleepFactor);
    tcdrain(port);
}


void Aflock::set_autoconfig(const int& port)
{
    ///////////////////////////////////////////////////////////////
    // FBB AUTO-CONFIGURATION (manual page 60)
    //
    // Must wait 300 milliseconds before and after this command
    ///////////////////////////////////////////////////////////////

    pickBird(1,port);

    sleep(1);

    char buff[3];
    buff[0] = 'P';
    buff[1] = 0x32;
    buff[2] = getNumberDevices();
    write(port, buff,3);

    sleep(1);

    tcdrain(port);

    int i=0;

#ifdef HAVE_AFLOCK_DEBUG
    pickBird(1,port);

    buff[0] = 'O';
    buff[1] = 0x32;
    write(port, buff,2);

    tcdrain(port);

    // TODO: we should know here, what's the current adressing mode     

    sleep(1);

    // we are in normal adressing mode, autoconfig command will return 5 byte
    const int infolength=5;
    char info[infolength];

    int numbytes;
    do {
       numbytes=read(port, info, infolength-i);
       if (numbytes<=0)
          {
          fprintf(stderr,"Aflock: cant get 5 bytes from autoconfig command:");
          fprintf(stderr,"Aflock running in normal mode ?\n");
          fprintf(stderr,"aflock: got %d bytes instead of 5\n",i);
          break;
          }
       } while ((i+=numbytes)<infolength);
    if (i==5) 
       {
       if (info[0]==0)
          fprintf(stderr,"Aflock: in standalone mode\n");
       else if (info[0]==1)
          fprintf(stderr,"Aflock: in transmitter/multiple sensors mode\n");
       else
          fprintf(stderr,"Aflock: unexpected returnvalue from auto-config\n");
       for (i=1;i<8;i++)
          if ( (info[1]&&(1<<i)) == 0)
             fprintf(stderr,"Aflock: device %d not running\n",i);
          else
             fprintf(stderr,"Aflock: device %d is running\n",i);
       for (i=0;i<7;i++)
          if ( (info[2]&&(1<<i)) == 0)
             fprintf(stderr,"Aflock: device %d not running\n",i+8);
          else
             fprintf(stderr,"Aflock: device %d is running\n",i+8);
       }
sleep(1);
#endif

}

/// not used 
void Aflock::set_group(const int& port)
{
    ////////////////////////////////////////////////////////////////
    // Setup group mode: (manual page 59)
    // 'P' Change Parameter
    // Number 35 (hex 23),
    // Set flag to 1 enabling group mode.
    ////////////////////////////////////////////////////////////////
    char buff[3];
    buff[0] = 'P';
    buff[1] = 0x23;
    buff[2] = 0x01;
    write(port, buff, 3);
    tcdrain(port);
    sleep(2);
}

void Aflock::set_sudden_output_change_lock(const int& port,bool lock)
{
    ////////////////////////////////////////////////////////////////
    // Set sudden output mode: (manual page ?)
    // 'P' Change Parameter
    // Number 14 (hex 0e),
    // Set flag to 1/0 to lock/unlock sudden output change
    ////////////////////////////////////////////////////////////////
    for (int i = 1; i <= getNumberDevices(); i++)
    {
       if (!deliverData(i))
          continue;
       pickBird( i,port );
       char buff[3];
       buff[0] = 'P';
       buff[1] = 0x0e;
       if (lock)
          buff[2] = 0x01;
       else
          buff[2] = 0x00;
       write(port, buff, 3);

       sleep(1);
       tcdrain(port);
    }
}



#else
// dummy function, cause some compilers do not like empty inputfiles....
static void Aflock_dummy(void) {}
#endif