Differences

This shows you the differences between two versions of the page.

--- sonar [2016/03/07 17:50]
zashi
+++ sonar [2016/11/09 15:12] (current)
zashi [Software]
@@ Line 149: / Line 149: @@
 Will test and update once I get some hardware. Once I verify the code works (or rather, once I test it and fix all the bugs I find) I'll restructure the code out into more manageable functions.
-<file c sonartest.c>
+<file c sonar.h>
-/* Compile: gcc -Wall -std=gnu11 -lportaudio -lm sonartest.c -o sonartest */
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <time.h>
-#include <math.h>
-#include <portaudio.h>
 #define MACH 343 // speed of sound in m/s
-#define ds 0.3 // distance between sensors in m (should be in equilateral triangle shape)
+/*#define ds 0.3 // distance between sensors in m (should be in equilateral triangle shape)*/
 #define ns_to_s(x) (x / 1000000000) // convert nanoseconds to seconds
-#define pi 3.14159265358979323 // ... pi. Like the constant, y'know?
+#define pi 3.14159265358979323846 // ... pi. Like the constant, y'know?
-typedef struct {
-	double x;
-	double y;
-} coord;
 /* relative positions of sensors */
@@ Line 178: / Line 165: @@
 #define RIGHTTOP 2
-/* actual positions of sensors *midpoints* in meters from origin point*/
+typedef struct {
+	double x;
+	double y;
+} coord;
+typedef struct {
+    int sensor_id[3];
+    PaStream *sensor_stream[2];
+    double ds;
+    int16_t sensor_data[2];
+	int16_t threshold;
+} sonar_config;
+void sonar_init (void);
+void sonar_listdev (void);
+void sonar_getloc ( coord *loc, sonar_config conf );
+int sonar_coord_cmp (coord a, coord b);
+</file>
+<file c sonar.c>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <time.h>
+#include <math.h>
+#include <portaudio.h>
+#include "sonar.h"
-void initaudiohardware (void)
+void sonar_init (void)
 {
 	/* init portaudio */
@@ Line 195: / Line 209: @@
 	atexit((void (*)(void))Pa_Terminate);
-	if ( 0 )
+}
-	{
+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;
@@ Line 215: / Line 271: @@
 			dev_info->defaultLowInputLatency );
 	}
-	}
 }
-extern inline int coord_cmp (coord a, coord b)
+int sonar_coord_cmp (coord a, coord b)
 {
 	return (a.x == b.x && a.y == b.y);
 }
-int main()
+void sonar_getloc ( coord *loc, sonar_config conf )
 {
-	coord midpoint[2];   /* array to store midpoints */
+	unsigned int i;
-	coord mp_order[2];   /* array to store the order of midpoints */
+	PaError err;
-	coord mp_map[2][2];  /* 2 dim array to make mapping of sensors to midpoint *much* easier */
+	coord midpoint[3];
-	coord loc;			 /* where the sound source is */
+	coord mp_order[3];
+	coord mp_map[3][3];
+	sonar_set_midpoints (midpoint, conf.ds);
-	/* Equilateral triangle 300 mm per side, with center of area on origin */
-	midpoint[TOPLEFT].x   = (-0.075);
-	midpoint[TOPLEFT].y   =  (0.042);
-	midpoint[RIGHTTOP].x  =  (0.075);
-	midpoint[RIGHTTOP].y  =  (0.042);
-	midpoint[LEFTRIGHT].x =  (0.0);
-	midpoint[LEFTRIGHT].y = (-0.088);
 	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];
-	PaError err;
+	PaStreamParameters sparams[2];
-	initaudiohardware();
-	PaStream *sensor_stream[2];
+	for ( i = 0; i < 3; i++ )
-	PaStreamParameters sparams[2];
-	struct timespec t[2];
-	sparams[0].device = 0;
-	sparams[0].channelCount = 1;
-	sparams[0].sampleFormat = paInt16;
-	sparams[0].suggestedLatency = 0;
-	sparams[0].hostApiSpecificStreamInfo = NULL;
-	if ( (err = Pa_OpenStream (
-			&sensor_stream[0],
-			&sparams[0],
-			NULL,
-,
-			paFramesPerBufferUnspecified,
-			paNoFlag,
-			NULL,
-			NULL )) )
 	{
-		printf ( "Failed to open stream: %s\n", Pa_GetErrorText ( err ) );
+		sparams[i].device = conf.sensor_id[i];
-		exit ( -2 );
+		sparams[i].channelCount = 1;
+		sparams[i].sampleFormat = paInt16;
+		sparams[i].suggestedLatency = 0;
+		sparams[i].hostApiSpecificStreamInfo = NULL;
 	}
-	int16_t sensor_data[2];
+	for ( i = 0; i < 3; i++ )
+		if ( (err = Pa_OpenStream (
+				&conf.sensor_stream[i],
+				&sparams[i],
+				NULL,
+,
+				paFramesPerBufferUnspecified,
+				paNoFlag,
+				NULL,
+				NULL )) )
+		{
+			printf ( "Failed to open stream for device %d: %s\n", conf.sensor_id[i], Pa_GetErrorText ( err ) );
+			exit ( -2 );
+		};
-	double theta[2];	// thetas and adjusted thetas
+	struct timespec t[3];
-	double b[2];		// y-intercepts
+	double theta[3];
-	int t_to_m[2];		// map timestamp to microphone
+	double b[3];
+	int t_to_m[3];
 	int order = 0;
-	for ( int i = 0; i < 1; i++ )
+	for ( i = 0; i < 3; i++ )
 		t[i].tv_sec = 0;
+	for ( i = 0; i < 3; i++ )
+		Pa_StartStream ( conf.sensor_stream[i] );
-	Pa_StartStream ( sensor_stream[0] );
 	for (;;)
 	{
-		for ( int i = 0; i < 1; i++ )
+		for ( i = 0; i < 3; i++ )
 		{
-			err = Pa_ReadStream ( sensor_stream[i], &sensor_data[i], 1);
+			err = Pa_ReadStream ( conf.sensor_stream[i], &conf.sensor_data[i], 1 );
-			if (! ( err == paInputOverflowed || err == paNoError))
+			if (! ( paInputOverflowed == err || err == paNoError ) )
 			{
-				printf ( "Failed to read stream: %s\n", Pa_GetErrorText ( err ) );
+				fprintf ( stderr, "Failed to read stream: %s\n", Pa_GetErrorText ( err ) );
 				exit ( -3 );
 			}
 		}
-		for ( int i = 0; i < 1; i++ )
+		for ( i = 0; i <= 2; i++ )
 		{
-			if ( abs( sensor_data[i] ) > 20000 && t[i].tv_sec != 0)
+			if ( abs( conf.sensor_data[i] ) > conf.threshold && t[i].tv_sec != 0 )
 			{
 				clock_gettime ( CLOCK_REALTIME, &t[i] );
@@ Line 314: / Line 356: @@
 		}
-		if (order > 2)
+		if ( order > 2 )
 		{
 			order = 0;
-			for ( int i = 0; i < 1; i++ )
+			for ( i = 0; i < 3; i++ )
 				t[i].tv_sec = 0;
 			/*
 			 *  Find Thetas
 			 */
-			/* find theta between t0 and t1 */
+			/* between t0 and t1 */
-			theta[0] = acos ( (ns_to_s(t[1].tv_nsec - t[0].tv_nsec) * MACH) / ds );
+			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) / ds );
+			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) / 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
+			 *  Map midpoints to sensors
 			 */
 			mp_order[0] = mp_map[t_to_m[0]][t_to_m[1]];
@@ Line 341: / Line 383: @@
 			/*
-			 * Adjust thetas depending on midpoint
+			 *  Adjust thetas depending on midpoint
 			 */
-			for ( int i = 0; i < 3; i++ )
+			for ( i = 0; i < 3; i++ )
 			{
-				if ( coord_cmp ( mp_order[i], midpoint[TOPLEFT] ) )
+				if ( sonar_coord_cmp ( mp_order[i], midpoint[TOPLEFT] ) )
 				{
-					theta[i] +=  pi / 3;
+					theta[i] += pi / 3;
 				}
-				else if ( coord_cmp ( mp_order[i], midpoint[RIGHTTOP] ) )
+				else if ( sonar_coord_cmp ( mp_order[i], midpoint[RIGHTTOP] ) )
 				{
 					theta[i] -= pi / 3;
 				}
 			}
 			/*
-			 * Determine b[]s (y-intercepts)
+			 *  Determine b[]s (y-intercepts)
 			 */
-			for ( int i = 0; i < 3; i++ )
+			for ( i = 0; i < 3; i++ )
 				b[i] = mp_order[i].y - tan(theta[i]) * mp_order[i].x;
 			/*
-			 * Determine X of loc
+			 *  Determine X of loc (TODO: Average together all loc.x)
 			 */
-			 loc.x = ( b[1] - b[0] ) / ( tan(theta[0]) - tan(theta[1]) );
+			loc->x = ( b[1] - b[0] ) / ( tan(theta[0]) - tan(theta[1]) );
 			/*
-			 * Determine Y of loc
+			 *  Determine Y of loc (TODO: Average together all loc.y)
 			 */
+			loc->y = tan(theta[0]) * loc->x + b[0];
-			 loc.y = tan(theta[0]) * loc.x + b[0];
-			 printf ( "LOCATION: %f meters x %f meters away from origin\n", loc.x, loc.y );
 		}
 	}
+}
+</file>
+<file c main.c>
+/* gcc -Wall -std=gnu11 -O2 -fno-aggressive-loop-optimizations -pedantic -lportaudio -lm *.c -o sonar */
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include "sonar.h"
+int main ( int argc, char **argv )
+{
+    int opt;
+    int sensor_id[3] = {-1, -1, -1};
+    int verbose=0;
+    sonar_config conf;
+    conf.ds = 0;
+    while ( (opt = getopt(argc, argv, "t:l:r:d:vs")) != -1 )
+    {
+        switch (opt)
+        {
+            /* specify sensor */
+            case 't':
+            case 'l':
+            case 'r':
+            	/* not clever, just lazy */
+                if ( 1 != sscanf(optarg, "%d", &sensor_id[(opt == 't') ? 0 : (opt == 'l') ? 1 : 2]) )
+                {
+                    perror("Failed to parse -<t|l|r> argument");
+                    exit(-1);
+                }
+                break;
+            case 'd': /* specify ds (distance between sensors) */
+                conf.ds = strtod(optarg, NULL);
+                if ( conf.ds > 343 )
+                {
+                    fprintf ( stderr, "Specified sensor distance is too large. Must be < 343\n" );
+                    exit(-1);
+                }
+                if (conf.ds <  0.1)
+                {
+                    fprintf ( stderr, "Specified sensor distance is too small. Must be > 0.1\n" );
+                    exit(-1);
+                }
+                break;
+            case 's': /* list audio input devices */
+                sonar_init();
+                sonar_listdev();
+                exit(0);
+                break;
+            case 'v':
+                verbose = 1;
+                break;
+            default:
+                //fprintf ( stderr, "Usage: %s -l | -s <sensor1> -s <sensor2> -s <sensor3> -d <distance in meters> \n", argv[0] );
+                exit(-1);
+        }
+    }
+    /* done processing arguments, make sure arguments make sense */
+    if ( -1 == sensor_id[0] || -1 == sensor_id[1] || -1 == sensor_id[2] )
+    {
+        fprintf ( stderr, "Error: 3 sensors, -t (top) -l (left) and -r (right) must be specified.\n" );
+        exit(-1);
+    }
+    if ( conf.ds == 0 )
+    {
+        fprintf ( stderr, "Error: Sensor distance (-d) must be set.\n" );
+        exit(-1);
+    }
+    if ( sensor_id[0] == sensor_id[1] || sensor_id[0] == sensor_id[2] || sensor_id[1] == sensor_id[2] )
+    {
+        fprintf ( stderr, "Error: Each sensor (-t,-l,-r) must be unique.\n" );
+        exit(-1);
+    }
+    if ( verbose )
+    {
+        printf ( "Sensors: %d, %d, and %d\n"
+                 "Distance between sensors: %fm",
+                 sensor_id[0], sensor_id[1], sensor_id[2],
+                 conf.ds
+               );
+    }
+    /* init audio hardware */
+    sonar_init();
+/*
+    sonar_openstreams(sensor_id);
+    sonar_
+*/
+    coord loc;
+    for (;;)
+    {
+        sonar_getloc(&loc, conf);
+        sleep(1);
+    }
+    return 0;
 }
 </file>

Differences

Search