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();
// }
// }
}
int initBluetooth(struct sserial_props *pProps, const char *btName, const char *btPin) {
char pTxBuff[32];
char pRxBuff[RX_BUFF_LENGTH] = {0};
int bSame, nLength;
LOGI("> Triger reset pin");
resetBluetooth(pProps);
LOGI("> Switch device to AT-mode");
ClrDtr(pProps);
SleepMs(1000);
#if 0
LOGV("> Reset to default");
WritePort(pProps, "AT+ORGL" CRLF, strlen("AT+ORGL" CRLF));
SleepMs(1000);
#endif
LOGV("> Switch to Slave mode");
WritePort(pProps, "AT+ROLE=0" CRLF, strlen("AT+ROLE=0" CRLF));
if (readLine(pProps, pRxBuff, OK_BUFF_LENGTH) <= 0 &&
strncasecmp(pRxBuff, "ok", 2)) {
LOGE("AT+ROLE failed to set (%s)", pRxBuff);
return 0;
}
if (getResult(pProps, "AT+NAME?" CRLF, pRxBuff, RX_BUFF_LENGTH)) {
bSame = strncmp(pRxBuff, btName, strlen(btName)) == 0;
// Get "OK"
readLine(pProps, pRxBuff, OK_BUFF_LENGTH);
if (bSame) {
LOGW("Bluetooth name is same and will not be changed");
goto skip_name;
}
}
LOGV("> Set name");
nLength = sprintf(pTxBuff, "AT+NAME=%s" CRLF, btName);
WritePort(pProps, pTxBuff, nLength);
if (readLine(pProps, pRxBuff, OK_BUFF_LENGTH) <= 0 &&
strncasecmp(pRxBuff, "ok", 2)) {
LOGE("AT+NAME failed to set (%s)", pRxBuff);
return 0;
}
skip_name:
if (getResult(pProps, "AT+PSWD?" CRLF, pRxBuff, RX_BUFF_LENGTH)) {
bSame = strncmp(pRxBuff, btPin, strlen(btPin)) == 0;
// Get "OK"
readLine(pProps, pRxBuff, OK_BUFF_LENGTH);
if (bSame) {
LOGW("Bluetooth pin code is same and will not be changed");
goto skip_password;
}
}
LOGV("> Set password");
nLength = sprintf(pTxBuff, "AT+PSWD=%s" CRLF, btPin);
WritePort(pProps, pTxBuff, nLength);
if (readLine(pProps, pRxBuff, OK_BUFF_LENGTH) <= 0 &&
strncasecmp(pRxBuff, "ok", 2)) {
LOGE("AT+PSWD failed to set (%s)", pRxBuff);
return 0;
}
skip_password:
LOGI("> Switch device to Data-mode");
SetDtr(pProps);
SleepMs(10);
resetBluetooth(pProps);
return 1;
}
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;
// }
// }
}
