int VSTest::init() {
char msg[256];
port_num = var_server_get_port();
hostest = var_server_get_hostname();
memset(&bob, '\0', sizeof(TCDevice));
bob.hostname = const_cast<char*>(hostest);
bob.port = port_num;
//std::cout << bob.hostname << bob.port << std::endl;
bob.disable_handshaking = TC_COMM_TRUE;
bob.disabled = TC_COMM_FALSE;
tc_connect(&bob);
if ( tc_isValid (&bob) ) {
printf ("connection is valid\n");
} else {
printf ("connection is NOT valid\n");
}
// default cycle rate
cycle_rate = 0.01;
sprintf(msg,"trick.var_cycle(%.2f)\n", cycle_rate);
vs_write(msg);
return (0);
}
void comm_read() {
int nbytes;
double packet[15];
if ( tc_isValid( &connection) && tc_pending( &connection)) {
nbytes = tc_read(& connection, (char*)&packet, sizeof(packet));
// Satellite Position
// satellite->pos[0] = packet[0];
// satellite->pos[1] = packet[1];
// satellite->pos[2] = packet[2];
satellite->pos[0] = 0.0;
satellite->pos[1] = 0.0;
satellite->pos[2] = 0.0;
planet->pos[0] = -packet[0];
planet->pos[1] = -packet[1];
planet->pos[2] = -packet[2];
// Satellite Velocity
satellite->vel[0] = packet[3];
satellite->vel[1] = packet[4];
satellite->vel[2] = packet[5];
// Vehicle Body to World Rotation Matrix
satellite->R[0][0] = packet[6];
satellite->R[0][1] = packet[7];
satellite->R[0][2] = packet[8];
satellite->R[1][0] = packet[9];
satellite->R[1][1] = packet[10];
satellite->R[1][2] = packet[11];
satellite->R[2][0] = packet[12];
satellite->R[2][1] = packet[13];
satellite->R[2][2] = packet[14];
// The camera position is relative to the satellite position.
satellite_camera->pos[0] = satellite->pos[0] + camera_range * sin(camera_azimuth * (PI/180.0));
satellite_camera->pos[1] = satellite->pos[1] + camera_range * cos(camera_azimuth * (PI/180.0));
satellite_camera->pos[2] = satellite->pos[2] + 0.0;
// The camera is looking at the satellite.
satellite_camera->tgt[0] = satellite->pos[0];
satellite_camera->tgt[1] = satellite->pos[1];
satellite_camera->tgt[2] = satellite->pos[2];
// Camera up is in the positive World-Z direction;
satellite_camera->up[0] = 0.0;
satellite_camera->up[1] = 0.0;
satellite_camera->up[2] = 1.0;
glutPostRedisplay();
}
}
/* Interupt signal handler. */
void sigint_hndlr( int sig )
{
fprintf(stderr,"client: disconnected...\n" );
if (tc_isValid(device))
tc_disconnect(device);
/* Exit the application. */
exit( 1 );
}
void move_ball() {
int nbytes ;
if ( tc_isValid(&connection) && tc_pending(&connection) ) {
nbytes = tc_read(&connection, (char*) &ball_position.x,
sizeof(double));
nbytes = tc_read(&connection, (char*) &ball_position.y,
sizeof(double));
}
glutPostRedisplay();
}
int main( int argc, char *argv[] ) {
int ctr;
BUFFER buffer;
int i, j;
int any_valid ;
pthread_t thread ;
int nbytes ;
if (argc >= 2) {
if (!strcmp(argv[1] , "-h")) {
fprintf(stderr,"usage: %s [tag_name]\n",argv[0]);
exit(0) ;
}
else
strcpy(other_tag , argv[1]) ;
}
init_attrBUFFER() ;
thread_id = pthread_self() ;
num_devices = 0 ;
#ifndef __WIN32__
/* Register the interupt signal handler. */
{
struct sigaction sigact; /* sigaction() sys call parm */
memset ( &sigact , 0 , sizeof(struct sigaction)) ;
sigact.sa_flags = 0;
sigact.sa_handler = (void(*)())sigint_hndlr;
if ( sigaction( SIGINT, &sigact, NULL ) < 0 )
perror("sigaction() failed for SIGINT");
#if (__sun)
sigact.sa_handler = (void(*)())stupid;
if ( sigaction( SIGALRM, &sigact, NULL ) < 0 )
perror("sigaction() failed for SIGALRM");
#endif
}
#endif
trick_error_init(&err_hndlr,
(TrickErrorFuncPtr)NULL,
(TrickErrorDataPtr)NULL,
TRICK_ERROR_TRIVIAL);
pthread_create(&thread , NULL, continuous_connect , NULL) ;
buffer.i1 = 0 ;
buffer.s1 = 1 ;
buffer.c1 = 2 ;
buffer.f1 = 3 ;
buffer.d1 = 4 ;
buffer.b1 = 8 ;
buffer.b2 = 7 ;
buffer.b3 = 43 ;
buffer.b4 = 357 ;
buffer.b5 = 1 ;
/*
buffer.b1 = 15 ;
buffer.b2 = 7 ;
buffer.b3 = 95 ;
buffer.b4 = 7853 ;
buffer.b5 = 6 ;
*/
ctr = 0;
while( 1 ) {
any_valid = 0 ;
/* write a message to all valid devices */
for ( i = 0 ; i < num_devices ; i++ ) {
if (tc_isValid(device[i])) {
nbytes = tc_write_byteswap(device[i],(char *)&buffer,sizeof(buffer),attrBUFFER);
if (nbytes != 0 ) {
fprintf(stderr,"server: wrote [%08x] to client %d\n",buffer.i1++, device[i]->socket);
any_valid = 1 ;
}
}
else {
/* free a device that is no longer valid and move all valid
devices down in the array to fill the hole */
free(device[i]);
for ( j = i ; j < (num_devices-1) ; j++ ) {
device[j] = device[j+1] ;
}
num_devices-- ;
i-- ;
}
}
if (any_valid) ctr++ ;
#ifdef __WIN32__
Sleep(500) ;
#else
usleep(500000) ;
#endif
}
} /* end main */
int main( int argc, char *argv[] ) {
int ctr;
int tc_rc;
BUFFER buffer ;
char other_tag[256] = "test_tag" ;
TrickErrorHndlr err_hndlr;
int *me ;
int status ;
if (argc >= 2) {
if (!strcmp(argv[1] , "-h")) {
fprintf(stderr,"usage: %s [tag_name]\n",argv[0]);
exit(0) ;
}
else {
strcpy(other_tag , argv[1]) ;
}
}
#ifndef __WIN32__
/* Register the interupt signal handler. */
{
struct sigaction sigact; /* sigaction() sys call parm */
memset ( &sigact , 0 , sizeof(struct sigaction)) ;
sigact.sa_flags = 0;
sigact.sa_handler = (void(*)())sigint_hndlr;
if ( sigaction( SIGINT, &sigact, NULL ) < 0 )
perror("sigaction() failed for SIGINT");
#if (__sun)
sigact.sa_handler = (void(*)())stupid;
if ( sigaction( SIGALRM, &sigact, NULL ) < 0 )
perror("sigaction() failed for SIGALRM");
#endif
}
#endif
memset((void *)&err_hndlr,'\0',sizeof(TrickErrorHndlr));
trick_error_init(&err_hndlr,
(TrickErrorFuncPtr)NULL,
(TrickErrorDataPtr)NULL,
TRICK_ERROR_TRIVIAL);
/* It is possible for multiple multiconnects to try and
initiate simultaneously causing some of the connects
to fail. This happens if all of the clients have the
same tag and are connecting to the same server. This is
the V.R. scenario. Continue trying to connect
until successful.
*/
do {
device = malloc(sizeof(TCDevice)) ;
memset((void *)device,'\0',sizeof(TCDevice));
status = tc_multiconnect(device , "client" , other_tag , &err_hndlr);
if (status != TC_SUCCESS) {
fprintf(stderr,"Error from tc_multiconnect... trying again\n");
}
} while (status != TC_SUCCESS) ;
ctr = 1;
me = ((int *)&buffer.f1 +1) ;
while( 1 ) {
while ( (tc_isValid(device)) && (!tc_pending(device)) )
#if __WIN32__
Sleep(1);
#else
usleep(1000);
#endif
if (tc_isValid(device)) {
tc_rc = tc_read(device,(char *)&buffer,sizeof(buffer));
if (tc_rc != 0) {
fprintf(stderr,"client: read msg %d from server:\n",ctr++);
fprintf(stderr," i1 = %08x, s1 = %d, c1 = %d, f1 = %f, d1 = %f\n",
buffer.i1, buffer.s1, buffer.c1, buffer.f1, buffer.d1 ) ;
fprintf(stderr," b1 = %d , b2 = %d, b3 = %d, b4 = %d, b5 = %d\n",
buffer.b1, buffer.b2, buffer.b3, buffer.b4, buffer.b5 ) ;
fprintf(stderr," me = %08x\n", *me ) ;
#if __WIN32__
Sleep(500) ;
#else
usleep(500000);
#endif
}
}
else {
fprintf(stderr,"Server side disconnected... exiting\n");
tc_disconnect(device);
break ;
}
}
} /* end main */
