iglasses.h

#ifndef iglasses_h
#define iglasses_h


typedef enum {Raw = 0, Cooked = 1, Euler = 2, CyberMaxx = 3} DataMode;
typedef enum {Polled = 'P', Continuous = 'C'} SendMode;
typedef enum {ASCII = 'A', Binary = 'B'} SendFormat;
typedef unsigned Filter;

inline float fixed2Float(short fixed)
{
     return fixed / (float) (1 << 14);
}

inline short float2Fixed(float fl)
{
     return (short) (fl * (1 << 14));
}

class EulerPacket
{
  public:
     float yaw() const
     {
	  return fixed2Float(_yaw);
     }

     float pitch() const
     {
	  return fixed2Float(_pitch);
     }
     
     float roll() const
     {
	  return fixed2Float(_roll);
     }

     void yaw(short newyaw)
     {
	  _yaw = newyaw;
     }

     void pitch(short newpitch)
     {
	  _pitch = newpitch;
     }

     void roll(short newroll)
     {
	  _roll = newroll;
     }
     friend ostream& operator<<(ostream &os, const EulerPacket &p);

  protected:
     short _yaw, _pitch, _roll;
};

ostream &operator<<(ostream &os, const EulerPacket &p);

int iglasses_init(int ttynum);
void iglasses_close();
int iglasses_reset(DataMode dm = Euler, SendMode sm = Polled, 
		   SendFormat sf = Binary, 
		   Filter mf = 3, Filter tf = 3);
int iglasses_getByte();
void iglasses_putByte(char byte);
void iglasses_flush();
void iglasses_putBytes(char const *str, int nChars);
int iglasses_getEulerPacket(EulerPacket &p);

#endif

iglasses.c++

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "iglasses.h"

ostream &operator<<(ostream &os, const EulerPacket &p)
{
     return os << p.yaw() << " " << p.pitch() << " " << p.roll();
}

#ifndef MIN
#define MIN(a,b) ((a)<(b)?(a):(b))
#endif
#ifndef MAX
#define MAX(a,b) ((a)>(b)?(a):(b))
#endif


int gTrackerPort = 0;

int iglasses_init(int ttynum)         /* param is which tty to use */
{
     /* MIN>0, TIME>0 -- time is intercharacter timer - at least 1
	MIN>0, TIME=0 -- read min characters
	MIN=0, TIME>0 -- TIME is timeout
	MIN=0, TIME=0 -- no waiting */

     char ttydev[] = "/dev/ttyd1";
     struct termios mode;

     if (ttynum>0 && ttynum<=9)
	  ttydev[strlen(ttydev)-1] = '0'+ttynum;
#ifdef DEBUG
     cerr << "Opening " << ttydev << endl;
#endif
     gTrackerPort = open(ttydev, O_RDWR);
     if (gTrackerPort < 0) 
     {
	  perror("init open");
	  return 1;
     }

     if (ioctl(gTrackerPort, TCGETA, &mode)==-1) {
	  perror("init ioctl(TCGETA)");
	  return 1;
     }
     mode.c_iflag = 0;
     mode.c_oflag = 0;
     mode.c_cflag = B9600 | CS8 | CREAD;
     mode.c_lflag = 0;
     mode.c_cc[VMIN] = 0;
     mode.c_cc[VTIME] = 1;
     if (ioctl(gTrackerPort, TCSETA, &mode)==-1) 
     {
	  perror("init ioctl(TCSETA)");
	  return 1;
     }

     return iglasses_reset();
}

void iglasses_close()
{
     close(gTrackerPort);
}

int iglasses_reset(DataMode dm, SendMode sm, SendFormat sf, 
		   Filter mf, Filter tf)
{
     iglasses_flush();
     iglasses_putBytes("!R\r", 3);
     if ('O' != iglasses_getByte())
	  return 1;

     char cmd[128];
     mf = MIN(mf, 7);
     tf = MIN(tf, 7);
     sprintf(cmd, "!M%d,%c,%c,%d,%d\r", dm, sm, sf, mf, tf);
#ifdef DEBUG
     cerr << "Reseting tracker with " << cmd << endl;
#endif
     iglasses_putBytes(cmd, strlen(cmd));
     if ('O' != iglasses_getByte())
	  return 1;
     return 0;
}

void iglasses_putByte(char byte)
{
#ifdef DEBUG
     cerr << "put [" << byte << "] = " << (int) byte << endl;
#endif
     write(gTrackerPort, &byte, 1);
}

int iglasses_getByte()
{
     int ret;
     char buf; 

     ret = read(gTrackerPort, &buf, 1);

     if (ret <= 0) 
	  return -1;
     return buf;
}

void iglasses_flush()
{
     //     ioctl(gTrackerPort, TCFLSH, 0);
}

void iglasses_putBytes(char const *str, int nChars)
{
     while (nChars-- > 0)
	  iglasses_putByte(*(str++));
}

int iglasses_getEulerPacket(EulerPacket &p)
{
     iglasses_flush();
     iglasses_putBytes("S", 1);
     
     if (0xff != iglasses_getByte())
	  return 1;

     unsigned long sum = 0xff;

     unsigned char ch[3][2];
     for (int ang = 0; ang < 3; ang++)
	  for (int b = 0; b < 2; b++)
	  {
	       int ret = iglasses_getByte();
	       if (ret < 0)
		    return 1;
	       else
		    sum += ch[ang][b] = ret;
	  }
     

     if ((0xff & sum) != iglasses_getByte())
     { 
	  cerr <<"Bad Checksum\n";
	  return 1;
     }

     p.yaw(*(short *) &ch[0]);
     p.pitch(*(short *) &ch[1]);
     p.roll(*(short *) &ch[2]);

     return 0;
}

main()
{     
     if (iglasses_init(2))
     {
	  cerr << "Couldn't initialize iglasses\n";
     	  exit(1);
     }

     //     iglasses_putBytes("!V\r");
     
     
     // Repeatedly poll the tracker for the orientation and print it out
     while (1)
     {
	  EulerPacket packet;
     
	  if (iglasses_getEulerPacket(packet))
	       cerr << "Bad packet\n";
	  else 
	       cerr << packet << endl;
     }

     iglasses_close();
}