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