main/old/iglasses.c

   1 /*
   2 THE COMPUTER CODE CONTAINED HEREIN IS THE SOLE PROPERTY OF PARALLAX
   3 SOFTWARE CORPORATION ("PARALLAX").  PARALLAX, IN DISTRIBUTING THE CODE TO
   4 END-USERS, AND SUBJECT TO ALL OF THE TERMS AND CONDITIONS HEREIN, GRANTS A
   5 ROYALTY-FREE, PERPETUAL LICENSE TO SUCH END-USERS FOR USE BY SUCH END-USERS
   6 IN USING, DISPLAYING,  AND CREATING DERIVATIVE WORKS THEREOF, SO LONG AS
   7 SUCH USE, DISPLAY OR CREATION IS FOR NON-COMMERCIAL, ROYALTY OR REVENUE
   8 FREE PURPOSES.  IN NO EVENT SHALL THE END-USER USE THE COMPUTER CODE
   9 CONTAINED HEREIN FOR REVENUE-BEARING PURPOSES.  THE END-USER UNDERSTANDS
  10 AND AGREES TO THE TERMS HEREIN AND ACCEPTS THE SAME BY USE OF THIS FILE.
  11 COPYRIGHT 1993-1999 PARALLAX SOFTWARE CORPORATION.  ALL RIGHTS RESERVED.
  12 */
  13
  14
  15 #pragma off (unreferenced)
  16 static char rcsid[] = "$Id: iglasses.c,v 1.1.1.1 2001-01-19 03:30:14 bradleyb Exp $";
  17 #pragma on (unreferenced)
  18
  19 #define DOS4G
  20
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <math.h>
  24 #include <conio.h>
  25
  26 #include "inferno.h"
  27 #include "error.h"
  28 #include "mono.h"
  29 #include "args.h"
  30 #include "text.h"
  31 #include "iglasses.h"
  32 #include "key.h"
  33 #include "commlib.h"
  34 #include "fast.h"
  35 #include "timer.h"
  36
  37 int iglasses_headset_installed=0;
  38
  39 void iglasses_close_tracking();
  40
  41 PORT * Iport = NULL;
  42
  43 #define USE_FILTERS 1
  44
  45 #ifdef USE_FILTERS
  46 #define FILTER_LENGTH 6
  47 typedef struct filter {
  48   fix history[FILTER_LENGTH];
  49   fix weights[FILTER_LENGTH];
  50   long len;
  51   fix * hCurrent,* hEnd,* hRestart;
  52 } filter;
  53
  54 void initFIR(filter * f);
  55 fix filterFIR(filter * f,fix newval);
  56 static filter X_filter, Y_filter, Z_filter;
  57 #endif
  58
  59 void iglasses_init_tracking(int serial_port)
  60 {
  61         fix t1,t2;
  62         int c;
  63
  64         if (iglasses_headset_installed) return;
  65
  66         if ( (serial_port < 1) || (serial_port > 4) )   {
  67                 Error( TXT_IGLASSES_ERROR_1 );
  68         }
  69         printf( "\n\n");
  70         printf( TXT_IGLASSES_INIT, serial_port );
  71         printf( "\n%s\n", TXT_IGLASSES_ON);
  72         printf( "Looking for glasses - %s", TXT_PRESS_ESC_TO_ABORT);
  73         Iport = PortOpenGreenleafFast(serial_port-1, 9600, 'N', 8, 1 );
  74         if ( !Iport )   {
  75                 printf( "%s\n", TXT_SERIAL_FAILURE, Iport->status );
  76                 return;
  77         }
  78
  79         SetDtr( Iport, 1 );
  80         SetRts( Iport, 1 );
  81         UseRtsCts( Iport, 0 );
  82
  83         t2 = timer_get_fixed_seconds() + i2f(20);
  84
  85         while(timer_get_fixed_seconds() < t2)   {
  86                 printf( "." );
  87                 t1 = timer_get_fixed_seconds() + F1_0;
  88                 ClearRXBuffer(Iport);
  89                 ClearTXBuffer(Iport);
  90                 WriteBuffer( Iport, "!\r", 2 );
  91                 while ( timer_get_fixed_seconds() < t1 )        {
  92                         if ( key_inkey() == KEY_ESC ) goto NotOK;
  93                         c = ReadChar( Iport );
  94                         if ( c == 'O' ) {
  95                                 goto TrackerOK1;
  96                         }
  97                 }
  98         }
  99
 100 NotOK:;
 101         printf( "\n\nWarning: Cannot find i-glasses! on port %d\n"
 102                                 " Press Esc to abort D2, any other key to continue without i-glasses support.\n"
 103                                 " Use SETUP to disable i-glasses support.\n",serial_port);
 104         if ( key_getch() == KEY_ESC )
 105                 exit(1);
 106         else
 107                 return;
 108
 109 TrackerOK1:
 110
 111         while( 1 )      {
 112                 printf( "." );
 113                 t1 = timer_get_fixed_seconds() + F1_0;
 114                 ClearRXBuffer(Iport);
 115                 ClearTXBuffer(Iport);
 116                 // M2 = p,b,h
 117                 // M1 = all data
 118                 WriteBuffer( Iport, "!M1,P,B\r", 8 );
 119                 while ( timer_get_fixed_seconds() < t1 )        {
 120                         if ( key_inkey() == KEY_ESC ) return;
 121                         c = ReadChar( Iport );
 122                         if ( c == 'O' ) {
 123                                 goto TrackerOK2;
 124                         }
 125                 }
 126         }
 127
 128 TrackerOK2:
 129
 130         printf( ".\n" );
 131         ClearRXBuffer(Iport);
 132         ClearTXBuffer(Iport);
 133
 134         WriteChar( Iport, 'S' );
 135
 136         iglasses_headset_installed = 1;
 137         atexit( iglasses_close_tracking );
 138
 139 #ifdef USE_FILTERS
 140         initFIR( &X_filter );
 141         initFIR( &Y_filter );
 142         initFIR( &Z_filter );
 143 #endif
 144
 145 //      {
 146 //              fix y,p,r;
 147 //              while( 1 )      {
 148 //                      iglasses_read_headset( &y, &p, &r);
 149 //                      //printf( "%d\t%d\t%d\n", y, p, r );
 150 //                      printf( "%8.2f\n", f2fl(y) );
 151 //                      if (key_inkey()==KEY_ESC) break;
 152 //              }
 153 //      }
 154
 155 }
 156
 157 void iglasses_close_tracking()  {
 158         if ( iglasses_headset_installed )       {
 159                 iglasses_headset_installed = 0;
 160                 PortClose(Iport);
 161                 Iport =  NULL;
 162         }
 163 }
 164
 165 //UNUSED int iglasses_read_headset1( fix *yaw, fix *pitch, fix *roll )
 166 //UNUSED {
 167 //UNUSED        int y,p,r;
 168 //UNUSED        static unsigned char buff[8];
 169 //UNUSED        unsigned char checksum;
 170 //UNUSED        int i,count;
 171 //UNUSED
 172 //UNUSED        ReadBufferTimed(Iport, buff, 8, 1000);
 173 //UNUSED        checksum = 0;
 174 //UNUSED        count = Iport->count;
 175 //UNUSED        for (i=0; i < 7; i++) checksum += buff[i];
 176 //UNUSED        if ( (count < 8) || (checksum != buff[7]) )     {
 177 //UNUSED                ClearRXBuffer(Iport);
 178 //UNUSED                WriteChar( Iport, 'S' );
 179 //UNUSED                *yaw = *pitch = *roll = 0;
 180 //UNUSED                return 0;
 181 //UNUSED        }
 182 //UNUSED        WriteChar( Iport, 'S' );
 183 //UNUSED
 184 //UNUSED        y  =  (short)(buff[1] << 8) | buff[2];
 185 //UNUSED        p  =  (short)(buff[3] << 8) | buff[4];
 186 //UNUSED        r  =  (short)(buff[5] << 8) | buff[6];
 187 //UNUSED
 188 //UNUSED        *yaw            = y;
 189 //UNUSED        *pitch  = p;
 190 //UNUSED        *roll   = r;
 191 //UNUSED
 192 //UNUSED        return 1;
 193 //UNUSED }
 194
 195 #define M_PI 3.14159265358979323846264338327950288
 196 #define FBITS 16384.
 197 #define TO_RADIANS (M_PI/FBITS)
 198 #define TO_DEGREES (180./M_PI)
 199 #define TO_FIX (0.5/M_PI)
 200
 201 fix i_yaw = 0;
 202 fix i_pitch = 0;
 203 fix i_roll = 0;
 204
 205 int Num_readings = 0;
 206 fix BasisYaw, BasisPitch, BasisBank;
 207
 208 int iglasses_read_headset( fix *yaw, fix *pitch, fix *roll )
 209 {
 210         static unsigned char buff[16];
 211         float radPitch,radRoll,sinPitch,sinRoll,cosPitch,cosRoll;
 212         float rotx,roty,rotz;
 213         float fx,fy,fz;
 214         long x,y,z,p,r;
 215         unsigned char checksum;
 216         long i,count;
 217
 218         ReadBufferTimed(Iport, buff, 12, 10 );                  // Wait 1/100 second for timeout.
 219         checksum = 0;
 220         count = Iport->count;
 221         ClearRXBuffer(Iport);
 222         WriteChar( Iport, 'S' );
 223         for (i=0; i < 11; i++) checksum += buff[i];
 224         if ( (count < 12) || (checksum != buff[11]) )   {
 225                 *yaw = i_yaw;
 226                 *pitch = i_pitch;
 227                 *roll = i_roll;
 228                 return 0;
 229         }
 230
 231         x  =  (short)(buff[1] << 8) | buff[2];
 232         y  =  (short)(buff[3] << 8) | buff[4];
 233         z  =  (short)(buff[5] << 8) | buff[6];
 234         p  =  (short)(buff[7] << 8) | buff[8];
 235         r  =  (short)(buff[9] << 8) | buff[10];
 236
 237         fx = (float)x/FBITS;
 238         fy = (float)y/FBITS;
 239         fz = (float)z/FBITS;
 240         radPitch = (float)p*TO_RADIANS;
 241         radRoll  = (float)r*TO_RADIANS;
 242         sinPitch = sin(radPitch);
 243         cosPitch = cos(radPitch);
 244         sinRoll  = sin(radRoll);
 245         cosRoll  = cos(radRoll);
 246
 247         roty = cosPitch*fy - sinPitch*fz;
 248         rotz = sinPitch*fy + cosPitch*fz;
 249         rotx = cosRoll*fx  - sinRoll*roty;
 250
 251 #ifdef USE_FILTERS
 252         *yaw   = filterFIR( &X_filter,fl2f(-atan2(rotz,rotx)*M_PI/2.0));
 253         *pitch = filterFIR( &Y_filter,fl2f(-radPitch*M_PI/2.0));
 254         *roll  = filterFIR( &Z_filter,fl2f(radRoll*M_PI/2.0));
 255         if ( Num_readings < 30 )        {
 256                 Num_readings++;
 257                 BasisYaw = *yaw;
 258                 BasisPitch = *pitch;
 259                 BasisBank = *roll;
 260         }
 261         *yaw -= BasisYaw;
 262         *pitch -= BasisPitch;
 263         *roll -= BasisBank;
 264 #else
 265         *yaw   = fl2f(-atan2(rotz,rotx)*M_PI/2.0);
 266         *pitch = fl2f(-radPitch*M_PI/2.0);
 267         *roll  = fl2f(radRoll*M_PI/2.0);
 268 #endif
 269
 270         i_yaw = *yaw;
 271         i_pitch = *pitch;
 272         i_roll = *roll;
 273
 274         return 1;
 275 }
 276
 277
 278 #ifdef USE_FILTERS
 279 void initWeights(filter * f)
 280 {
 281         fix sum;
 282         long i;
 283
 284         // Generate weights.
 285         sum = 0;
 286         for (i=0; i < f->len; i++) {
 287                 //f->weights[i] = i;            // Linear ramp
 288                 //f->weights[i] = i*i;          // Exp ramp
 289                 f->weights[i] = F1_0;           // Average
 290
 291         }
 292
 293         // Summate.
 294         for (i=0; i < f->len; i++) {
 295                 sum += f->weights[i];
 296         }
 297
 298         // Normalize and convert to fixed point.
 299         for (i=0; i < f->len; i++) {
 300                 f->weights[i] = fixdiv(f->weights[i],sum);
 301         }
 302 }
 303
 304 void initHistory(filter * f)
 305 {
 306         long i;
 307         for (i=0; i < f->len; i++) {
 308                 f->history[i] = F1_0;
 309         }
 310         f->hCurrent = f->history;
 311         f->hEnd     = &f->history[f->len-1];
 312         f->hRestart = &f->history[-1];
 313 }
 314
 315 void initFIR(filter * f)
 316 {
 317   f->len = FILTER_LENGTH;
 318   initWeights(f);
 319   initHistory(f);
 320 }
 321
 322 fix filterFIR(filter * f,fix newval)
 323 {
 324         fix * currp,* last;
 325         fix * weightp;
 326         fix sum;
 327
 328         currp   = f->hCurrent;
 329         last    = f->hCurrent;
 330         weightp = f->weights;
 331
 332         *(f->hCurrent)++ = newval;
 333         if (f->hCurrent == &f->history[f->len]) f->hCurrent = f->history;
 334
 335         // Compute FIR filter
 336         sum = 0;
 337         while (1) {
 338                 sum += fixmul( (*currp--), (*weightp++) );
 339         if (currp == f->hRestart) currp = f->hEnd;
 340         if (currp == last) break;
 341         } /* while */
 342         return sum/f->len;
 343 } /* filterFIR */
 344
 345 #endif