void main()
{
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize I/O and Peripherals for application */
InitApp();
SendMessage("READY");
while(1)
{
if (RCIF)
{
if (FERR)
{
rxchar = RCREG;
ClearRXBuffer();
SendMessage(UART_FRAMING_ERROR); // Framing Error - clear with a read
}
else
{
rxchar = RCREG; // recive char
if(rxpos < RXBUFFERSIZE)
{
rxbuffer[rxpos++] = rxchar;
if ((rxpos == CMD_SIZE) & (rxchar == 0x0D)) // CR recived - do command
{
rxchar = DoCommand();
ClearRXBuffer();
SendMessage(rxchar ? CMD_ACK : CMD_ERROR); // command acknowledge
}
}
else
{
ClearRXBuffer();
SendMessage(BUFFER_FULL_ERROR); // buffer full error
}
}
}
if (OERR)
{
CREN = 0;
rxchar = RCREG;
rxchar = RCREG;
ClearRXBuffer();
SendMessage(UART_OVERFLOW_ERROR); // rx overrun error
CREN = 1;
}
}
}
void victor_read_headset_raw( fix *yaw, fix *pitch, fix *roll )
{
int y, p, r, count;
static unsigned char buff[6];
ReadBufferTimed(VictorPort, buff, 6, 10);
count = VictorPort->count;
if ( count < 6 ) {
ClearRXBuffer(VictorPort);
WriteChar( VictorPort, 'S' );
*yaw = v_yaw;
*pitch = v_pitch;
*roll = v_roll;
return;
}
WriteChar( VictorPort, 'S' );
y = (short)(buff[0] << 8) | buff[1];
p = (short)(buff[2] << 8) | buff[3];
r = (short)(buff[4] << 8) | buff[5];
v_yaw = *yaw = y;
v_pitch = *pitch = -1*(p - 16384);
v_roll = *roll = -1*(r - 16384);
}
void victor_init_tracking(int serial_port)
{
if (Victor_headset_installed) return;
printf( "%s %d\n", TXT_INIT_VICTOR, serial_port );
if ( (serial_port < 1) || (serial_port > 4) ) {
Error( "Invalid serial port parameter for -maxxtrak!\n" );
}
VictorPort = PortOpenGreenleafFast(serial_port-1, 19200, 'N', 8, 1 );
if ( !VictorPort ) {
printf( "%s %d\n", TXT_SERIAL_FAILURE, VictorPort->status );
return;
}
// printf( "Port opened\n" );
SetDtr( VictorPort, 1 );
// printf( "Step #1\n" );
SetRts( VictorPort, 1 );
// printf( "Step #2\n" );
UseRtsCts( VictorPort, 0 );
// printf( "Step #3\n" );
Victor_headset_installed = 1;
atexit( victor_close_tracking );
WriteChar( VictorPort, 'A' ); // Absolute coordinate mode
WriteChar( VictorPort, 'C' ); // High accuracy mode
WriteChar( VictorPort, 'F' ); // Binary Mode
WriteChar( VictorPort, 'G' ); // Sampling upon request
ClearRXBuffer( VictorPort );
WriteChar( VictorPort, 'S' ); // Request a packet
// {
// int k=0;
// while ( k!=KEY_ESC ) {
// fix y, p, r;
// victor_read_headset_filtered( &y, &p, &r );
// printf( "%d\t%d\t%d\n", y, p, r );
// k = key_inkey();
// }
// }
}
void iglasses_init_tracking(int serial_port)
{
fix t1,t2;
int c;
if (iglasses_headset_installed) return;
if ( (serial_port < 1) || (serial_port > 4) ) {
Error( TXT_IGLASSES_ERROR_1 );
}
printf( "\n\n");
printf( TXT_IGLASSES_INIT, serial_port );
printf( "\n%s\n", TXT_IGLASSES_ON);
printf( "Looking for glasses - %s", TXT_PRESS_ESC_TO_ABORT);
Iport = PortOpenGreenleafFast(serial_port-1, 9600, 'N', 8, 1 );
if ( !Iport ) {
printf( "%s\n", TXT_SERIAL_FAILURE, Iport->status );
return;
}
SetDtr( Iport, 1 );
SetRts( Iport, 1 );
UseRtsCts( Iport, 0 );
t2 = timer_get_fixed_seconds() + i2f(20);
while(timer_get_fixed_seconds() < t2) {
printf( "." );
t1 = timer_get_fixed_seconds() + F1_0;
ClearRXBuffer(Iport);
ClearTXBuffer(Iport);
WriteBuffer( Iport, "!\r", 2 );
while ( timer_get_fixed_seconds() < t1 ) {
if ( key_inkey() == KEY_ESC ) goto NotOK;
c = ReadChar( Iport );
if ( c == 'O' ) {
goto TrackerOK1;
}
}
}
NotOK:;
printf( "\n\nWarning: Cannot find i-glasses! on port %d\n"
" Press Esc to abort D2, any other key to continue without i-glasses support.\n"
" Use SETUP to disable i-glasses support.\n",serial_port);
if ( key_getch() == KEY_ESC )
exit(1);
else
return;
TrackerOK1:
while( 1 ) {
printf( "." );
t1 = timer_get_fixed_seconds() + F1_0;
ClearRXBuffer(Iport);
ClearTXBuffer(Iport);
// M2 = p,b,h
// M1 = all data
WriteBuffer( Iport, "!M1,P,B\r", 8 );
while ( timer_get_fixed_seconds() < t1 ) {
if ( key_inkey() == KEY_ESC ) return;
c = ReadChar( Iport );
if ( c == 'O' ) {
goto TrackerOK2;
}
}
}
TrackerOK2:
printf( ".\n" );
ClearRXBuffer(Iport);
ClearTXBuffer(Iport);
WriteChar( Iport, 'S' );
iglasses_headset_installed = 1;
atexit( iglasses_close_tracking );
#ifdef USE_FILTERS
initFIR( &X_filter );
initFIR( &Y_filter );
initFIR( &Z_filter );
#endif
// {
// fix y,p,r;
// while( 1 ) {
// iglasses_read_headset( &y, &p, &r);
// //printf( "%d\t%d\t%d\n", y, p, r );
// printf( "%8.2f\n", f2fl(y) );
// if (key_inkey()==KEY_ESC) break;
// }
// }
}
int iglasses_read_headset( fix *yaw, fix *pitch, fix *roll )
{
static unsigned char buff[16];
float radPitch,radRoll,sinPitch,sinRoll,cosPitch,cosRoll;
float rotx,roty,rotz;
float fx,fy,fz;
long x,y,z,p,r;
unsigned char checksum;
long i,count;
ReadBufferTimed(Iport, buff, 12, 10 ); // Wait 1/100 second for timeout.
checksum = 0;
count = Iport->count;
ClearRXBuffer(Iport);
WriteChar( Iport, 'S' );
for (i=0; i < 11; i++) checksum += buff[i];
if ( (count < 12) || (checksum != buff[11]) ) {
*yaw = i_yaw;
*pitch = i_pitch;
*roll = i_roll;
return 0;
}
x = (short)(buff[1] << 8) | buff[2];
y = (short)(buff[3] << 8) | buff[4];
z = (short)(buff[5] << 8) | buff[6];
p = (short)(buff[7] << 8) | buff[8];
r = (short)(buff[9] << 8) | buff[10];
fx = (float)x/FBITS;
fy = (float)y/FBITS;
fz = (float)z/FBITS;
radPitch = (float)p*TO_RADIANS;
radRoll = (float)r*TO_RADIANS;
sinPitch = sin(radPitch);
cosPitch = cos(radPitch);
sinRoll = sin(radRoll);
cosRoll = cos(radRoll);
roty = cosPitch*fy - sinPitch*fz;
rotz = sinPitch*fy + cosPitch*fz;
rotx = cosRoll*fx - sinRoll*roty;
#ifdef USE_FILTERS
*yaw = filterFIR( &X_filter,fl2f(-atan2(rotz,rotx)*M_PI/2.0));
*pitch = filterFIR( &Y_filter,fl2f(-radPitch*M_PI/2.0));
*roll = filterFIR( &Z_filter,fl2f(radRoll*M_PI/2.0));
if ( Num_readings < 30 ) {
Num_readings++;
BasisYaw = *yaw;
BasisPitch = *pitch;
BasisBank = *roll;
}
*yaw -= BasisYaw;
*pitch -= BasisPitch;
*roll -= BasisBank;
#else
*yaw = fl2f(-atan2(rotz,rotx)*M_PI/2.0);
*pitch = fl2f(-radPitch*M_PI/2.0);
*roll = fl2f(radRoll*M_PI/2.0);
#endif
i_yaw = *yaw;
i_pitch = *pitch;
i_roll = *roll;
return 1;
}
