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.
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)
37 int iglasses_headset_installed=0;
39 void iglasses_close_tracking();
46 #define FILTER_LENGTH 6
47 typedef struct filter {
48 fix history[FILTER_LENGTH];
49 fix weights[FILTER_LENGTH];
51 fix * hCurrent,* hEnd,* hRestart;
54 void initFIR(filter * f);
55 fix filterFIR(filter * f,fix newval);
56 static filter X_filter, Y_filter, Z_filter;
59 void iglasses_init_tracking(int serial_port)
64 if (iglasses_headset_installed) return;
66 if ( (serial_port < 1) || (serial_port > 4) ) {
67 Error( TXT_IGLASSES_ERROR_1 );
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 );
75 printf( "%s\n", TXT_SERIAL_FAILURE, Iport->status );
81 UseRtsCts( Iport, 0 );
83 t2 = timer_get_fixed_seconds() + i2f(20);
85 while(timer_get_fixed_seconds() < t2) {
87 t1 = timer_get_fixed_seconds() + F1_0;
90 WriteBuffer( Iport, "!\r", 2 );
91 while ( timer_get_fixed_seconds() < t1 ) {
92 if ( key_inkey() == KEY_ESC ) goto NotOK;
93 c = ReadChar( Iport );
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 )
113 t1 = timer_get_fixed_seconds() + F1_0;
114 ClearRXBuffer(Iport);
115 ClearTXBuffer(Iport);
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 );
131 ClearRXBuffer(Iport);
132 ClearTXBuffer(Iport);
134 WriteChar( Iport, 'S' );
136 iglasses_headset_installed = 1;
137 atexit( iglasses_close_tracking );
140 initFIR( &X_filter );
141 initFIR( &Y_filter );
142 initFIR( &Z_filter );
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;
157 void iglasses_close_tracking() {
158 if ( iglasses_headset_installed ) {
159 iglasses_headset_installed = 0;
165 //UNUSED int iglasses_read_headset1( fix *yaw, fix *pitch, fix *roll )
168 //UNUSED static unsigned char buff[8];
169 //UNUSED unsigned char checksum;
170 //UNUSED int i,count;
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;
182 //UNUSED WriteChar( Iport, 'S' );
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];
195 #define M_PI 3.14159265358979323846264338327950288
197 #define TO_RADIANS (M_PI/FBITS)
198 #define TO_DEGREES (180./M_PI)
199 #define TO_FIX (0.5/M_PI)
205 int Num_readings = 0;
206 fix BasisYaw, BasisPitch, BasisBank;
208 int iglasses_read_headset( fix *yaw, fix *pitch, fix *roll )
210 static unsigned char buff[16];
211 float radPitch,radRoll,sinPitch,sinRoll,cosPitch,cosRoll;
212 float rotx,roty,rotz;
215 unsigned char checksum;
218 ReadBufferTimed(Iport, buff, 12, 10 ); // Wait 1/100 second for timeout.
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]) ) {
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];
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);
247 roty = cosPitch*fy - sinPitch*fz;
248 rotz = sinPitch*fy + cosPitch*fz;
249 rotx = cosRoll*fx - sinRoll*roty;
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 ) {
262 *pitch -= BasisPitch;
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);
279 void initWeights(filter * f)
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
294 for (i=0; i < f->len; i++) {
295 sum += f->weights[i];
298 // Normalize and convert to fixed point.
299 for (i=0; i < f->len; i++) {
300 f->weights[i] = fixdiv(f->weights[i],sum);
304 void initHistory(filter * f)
307 for (i=0; i < f->len; i++) {
308 f->history[i] = F1_0;
310 f->hCurrent = f->history;
311 f->hEnd = &f->history[f->len-1];
312 f->hRestart = &f->history[-1];
315 void initFIR(filter * f)
317 f->len = FILTER_LENGTH;
322 fix filterFIR(filter * f,fix newval)
330 weightp = f->weights;
332 *(f->hCurrent)++ = newval;
333 if (f->hCurrent == &f->history[f->len]) f->hCurrent = f->history;
335 // Compute FIR filter
338 sum += fixmul( (*currp--), (*weightp++) );
339 if (currp == f->hRestart) currp = f->hEnd;
340 if (currp == last) break;