Re> Polhemus FASTRAK

Cedric Gautier (gce++at++syseca.fr)
Fri, 14 Apr 95 10:49:17 +0200

• Next message: Cedric Gautier: "Re> Polhemus FASTRAK"
• Previous message: Kent Miller: "Polhemus FASTRAK"

answer:
I think one of the best way to integrate a Fastrak in a Performer app
is to make a separate process which will communicate with the main
Performer app through shared memory with DPool management ! ...
Hereafter is a complete code of this separate procees I made for my
own tests ...

#include <stdio.h>
#include <fcntl.h>
#include <termio.h>
#include <signal.h>
#include <math.h>
#include <Performer/pf.h>
#include <Performer/pr.h>

#define STV_POOL_ID 1
#define STV_POOL_SIZE 256
#define STV_POOL_NAME "fastrak"

static int fast_fd;
static pfDataPool *stvDataPool;
static float *stvPoolMem, *stvPoolPtr;

void abort()
{
 pfReleaseDPool(stvDataPool);
 close(fast_fd);
 exit(0);
}

main(int argc, char **argv)
{
 struct termio arg;
 float x1, y1, z1, h1, p1, r1;
 float x2, y2, z2, h2, p2, r2;
 char buf[50], c1, c2, c3;
 int v= 0;
  
 signal(SIGINT, abort);

 if (argc>1)
         if (strcmp(argv[1], "+")==0)
                v= 1;

 while ((stvDataPool= pfAttachDPool(STV_POOL_NAME))==NULL)
        fprintf(stderr, "\t- Data Pool Attach ...\r");
 stvPoolMem= (float *)pfDPoolFind(stvDataPool, STV_POOL_ID);
 fprintf(stderr, "\t- Ready \n");
  
 if ((fast_fd= open("/dev/ttyd1", O_RDWR))== -1)
        perror("open");
 if (ioctl(fast_fd, TCGETA, &arg)== -1)
        perror("geta");
 arg.c_cflag = B38400| CS7| PARENB| CREAD| CLOCAL| HUPCL;
 arg.c_iflag= 0;
 arg.c_lflag= ICANON;
 arg.c_oflag= 0;
 arg.c_line= 0;
 arg.c_cc[VTIME]= 0;
 arg.c_cc[VMIN]= 0;
 if (ioctl(fast_fd, TCSETA, &arg)== -1)
         perror("seta");

 if (write(fast_fd, "u", 1)!= 1)
        fprintf(stderr, "write\n");
 if (write(fast_fd, "H1,0,0,1\r\n", 10)!= 10)
        fprintf(stderr, "write\n");
 if (write(fast_fd, "H2,0,0,1\r\n", 10)!= 10)
        fprintf(stderr, "write\n");

 while(1) {

 if (write(fast_fd, "P", 1)!= 1)
        fprintf(stderr, "ask\n");

 if (read(fast_fd, buf, 47)!= 47)
        fprintf(stderr, "read\n");
 buf[46]= '\0';
 sscanf(buf, "%c%c%c%f%f%f%f%f%f", &c1, &c2, &c3, &x1, &y1, &z1, &h1, &p1, &r1);
 if (c3!=' ')
        fprintf(stderr, "\t- Warning [%c] On Tracker 1\n", c3);
 if (v) {
         fprintf(stderr, "RAW: %s\n", buf);
         fprintf(stderr, "COD: %c%c%c [ %3.2f %3.2f %3.2f ][ %3.2f %3.2f %3.2f ]\n",
                c1, c2, c3, x1, y1, z1, h1, p1, r1);
         fflush(stderr);
 }
 if (read(fast_fd, buf, 47)!= 47)
        fprintf(stderr, "read\n");
 buf[46]= '\0';
 sscanf(buf, "%c%c%c%f%f%f%f%f%f", &c1, &c2, &c3, &x2, &y2, &z2, &h2, &p2, &r2);
 if (c3!=' ')
        fprintf(stderr, "\t- Warning [%c] On Tracker 2\n", c3);
 if (v) {
         fprintf(stderr, "RAW: %s\n", buf);
         fprintf(stderr, "COD: %c%c%c [ %3.2f %3.2f %3.2f ][ %3.2f %3.2f %3.2f ]\n",
                c1, c2, c3, x2, y2, z2, h2, p2, r2);
         fflush(stderr);
 }
 pfDPoolLock(stvPoolMem);
 stvPoolPtr= stvPoolMem;
 *stvPoolPtr++= fabsf(x1);
 *stvPoolPtr++= fabsf(y1);
 *stvPoolPtr++= fabsf(z1);
 *stvPoolPtr++= h1;
 *stvPoolPtr++= p1;
 *stvPoolPtr++= r1;
 *stvPoolPtr++= fabsf(x2);
 *stvPoolPtr++= fabsf(y2);
 *stvPoolPtr++= fabsf(z2);
 *stvPoolPtr++= h2;
 *stvPoolPtr++= p2;
 *stvPoolPtr++= r2;
 pfDPoolUnlock(stvPoolMem);

 }
}

Note: this is done for two trackers and it should be usefull to get datas
in binary mode instead of ascii one ! ...

Good Luck ! ...

Cedric
Thomson Group
email: gce++at++syseca.fr

• Next message: Cedric Gautier: "Re> Polhemus FASTRAK"
• Previous message: Kent Miller: "Polhemus FASTRAK"

This archive was generated by hypermail 2.0b2 on Mon Aug 10 1998 - 17:51:17 PDT

This message has been cleansed for anti-spam protection. Replace '++at++' in any mail addresses with the '@' symbol.