#include #include #include #include #include #include #include "sonar.h" void sonar_init (void) { /* init portaudio */ PaError err; if ( (err = Pa_Initialize()) ) { printf ( "Failed to init: %s\n", Pa_GetErrorText ( err ) ); exit ( -1 ); } /* uh.. sure */ atexit((void (*)(void))Pa_Terminate); } void sonar_set_midpoints (coord *mps, double l) { /* determine the midpoints of a triangle of length l. * This assumes the triangle is equilateral and centered * on the origin point. */ double h_bigt; /* height of big triangle */ double r_bigt; /* radius of big triangle */ /* small triangle = triangle formed by midpoints of big triangle */ double r_smallt; /* radius of small triangle */ double l_smallt; /* length of side of small t */ double m; /* gives midpoint y coord. */ /* use pythagoras to determine height of equilateral triangle */ h_bigt = sqrt( pow(l, 2) - pow( l/2, 2) ); /* radius of an equilateral triangle = length of side / sqrt(3) */ r_bigt = l / sqrt(3); /* the difference in the height of bigt and its radius gives radius of smallt */ r_smallt = h_bigt - r_bigt; l_smallt = sqrt(3) * r_smallt; /* use pythagoras again */ m = sqrt( pow(r_smallt, 2) - pow( l_smallt / 2, 2) ); mps[TOPLEFT].x = -l_smallt / 2; mps[TOPLEFT].y = m; mps[RIGHTTOP].x = l_smallt / 2; mps[RIGHTTOP].y = m; mps[LEFTRIGHT].x = 0; mps[LEFTRIGHT].y = -r_smallt; } void sonar_listdev (void) { /* Figure out input devices we have */ PaDeviceIndex dev; const PaDeviceInfo *dev_info; for ( dev = 0; dev < Pa_GetDeviceCount(); dev++ ) { dev_info = Pa_GetDeviceInfo ( dev ); printf ( "Index: %d\n" "Name: %s\n" "Input Channels: %d\n" "Sample Rate: %f\n" "Default Low Input Latency: %f\n" "\n", dev, dev_info->name, dev_info->maxInputChannels, dev_info->defaultSampleRate, dev_info->defaultLowInputLatency ); } } int sonar_coord_cmp (coord a, coord b) { return (a.x == b.x && a.y == b.y); } void sonar_getloc ( coord *loc, sonar_config conf ) { unsigned int i; PaError err; coord midpoint[3]; coord mp_order[3]; coord mp_map[3][3]; sonar_set_midpoints (midpoint, conf.ds); mp_map[LEFT][RIGHT] = midpoint[LEFTRIGHT]; mp_map[RIGHT][LEFT] = midpoint[LEFTRIGHT]; mp_map[TOP][LEFT] = midpoint[TOPLEFT]; mp_map[LEFT][TOP] = midpoint[TOPLEFT]; mp_map[RIGHT][TOP] = midpoint[RIGHTTOP]; mp_map[TOP][RIGHT] = midpoint[RIGHTTOP]; PaStreamParameters sparams[2]; for ( i = 0; i < 3; i++ ) { sparams[i].device = conf.sensor_id[i]; sparams[i].channelCount = 1; sparams[i].sampleFormat = paInt16; sparams[i].suggestedLatency = 0; sparams[i].hostApiSpecificStreamInfo = NULL; } for ( i = 0; i < 3; i++ ) if ( (err = Pa_OpenStream ( &conf.sensor_stream[i], &sparams[i], NULL, 44100, paFramesPerBufferUnspecified, paNoFlag, NULL, NULL )) ) { printf ( "Failed to open stream for device %d: %s\n", conf.sensor_id[i], Pa_GetErrorText ( err ) ); exit ( -2 ); }; struct timespec t[3]; double theta[3]; double b[3]; int t_to_m[3]; int order = 0; for ( i = 0; i < 3; i++ ) t[i].tv_sec = 0; for ( i = 0; i < 3; i++ ) Pa_StartStream ( conf.sensor_stream[i] ); for (;;) { for ( i = 0; i < 3; i++ ) { err = Pa_ReadStream ( conf.sensor_stream[i], &conf.sensor_data[i], 1 ); if (! ( paInputOverflowed == err || err == paNoError ) ) { fprintf ( stderr, "Failed to read stream: %s\n", Pa_GetErrorText ( err ) ); exit ( -3 ); } } for ( i = 0; i <= 2; i++ ) { if ( abs( conf.sensor_data[i] ) > conf.threshold && t[i].tv_sec != 0 ) { clock_gettime ( CLOCK_REALTIME, &t[i] ); t_to_m[order++] = i; } } if ( order > 2 ) { order = 0; for ( i = 0; i < 3; i++ ) t[i].tv_sec = 0; /* * Find Thetas */ /* between t0 and t1 */ theta[0] = acos ( (ns_to_s(t[1].tv_nsec - t[0].tv_nsec) * MACH) / conf.ds ); /* between t1 and t2 */ theta[1] = acos ( (ns_to_s(t[2].tv_nsec - t[1].tv_nsec) * MACH) / conf.ds ); /* between t0 and t2 */ theta[2] = acos ( (ns_to_s(t[2].tv_nsec - t[0].tv_nsec) * MACH) / conf.ds ); /* * Map midpoints to sensors */ mp_order[0] = mp_map[t_to_m[0]][t_to_m[1]]; mp_order[1] = mp_map[t_to_m[1]][t_to_m[2]]; mp_order[2] = mp_map[t_to_m[0]][t_to_m[2]]; /* * Adjust thetas depending on midpoint */ for ( i = 0; i < 3; i++ ) { if ( sonar_coord_cmp ( mp_order[i], midpoint[TOPLEFT] ) ) { theta[i] += pi / 3; } else if ( sonar_coord_cmp ( mp_order[i], midpoint[RIGHTTOP] ) ) { theta[i] -= pi / 3; } } /* * Determine b[]s (y-intercepts) */ for ( i = 0; i < 3; i++ ) b[i] = mp_order[i].y - tan(theta[i]) * mp_order[i].x; /* * Determine X of loc (TODO: Average together all loc.x) */ loc->x = ( b[1] - b[0] ) / ( tan(theta[0]) - tan(theta[1]) ); /* * Determine Y of loc (TODO: Average together all loc.y) */ loc->y = tan(theta[0]) * loc->x + b[0]; } } }