Diff: medeltest.cpp

Revision:

5:3912347f8b4b

Parent:

3:c6b26e356c10

Child:

6:73617748bb3a

--- a/medeltest.cpp	Thu May 11 11:03:10 2017 +0000
+++ b/medeltest.cpp	Thu May 11 12:17:28 2017 +0000
@@ -28,6 +28,7 @@
 #define Debug(x) x
 #define DebugPrintState(y) y
 #define DebugArcSin(z) z
+#define DebugPrintDelay(q) q
 
 
 //----------VARIABLES HERE
@@ -44,6 +45,7 @@
 int positionOfMaxVal_1;
 int positionOfMaxVal_2;
 const double PI = 3.14159265358979323846;
+double timedelay = 0;
 
 // State machine
 int    STATE;
@@ -281,32 +283,45 @@
 
             case TESTNEW:
                 DebugPrintState( std::cout << "Nucleo state is TESTNEW: " << std::endl; );
-                int i = 0;
-                bool quit = false;
-                while(!quit) {
-                    channel_1[i] = getAudioValue(mic1);
-                    timestamps_1[i] = t.read_us();
-                    channel_2[i] = getAudioValue(mic2);
-                    timestamps_2[i] = t.read_us();
-                    if(overThreshold(channel_1[i], channel_2[i]) == true) {
-                        capture_1[0] = channel_1[i];
-                        capturestamps_1[0] = timestamps_1[i];
-                        capture_2[0] = channel_2[i];
-                        capturestamps_2[0] = timestamps_2[i];
-                        for(int i = 1; i < captureLength; i++) {
-                            capture_1[i] = getAudioValue(mic1);
-                            capturestamps_1[i] = t.read_us();
-                            capture_2[i] = getAudioValue(mic2);
-                            capturestamps_2[i] = t.read_us();
+                
+                double avgTimedelay = 0;
+                int counter = 0;
+                while(counter<1000){
+                    int i = 0;
+                    bool quit = false;
+                    while(!quit) {
+                        channel_1[i] = getAudioValue(mic1);
+                        timestamps_1[i] = t.read_us();
+                        channel_2[i] = getAudioValue(mic2);
+                        timestamps_2[i] = t.read_us();
+                        if(overThreshold(channel_1[i], channel_2[i]) == true) {
+                            capture_1[0] = channel_1[i];
+                            capturestamps_1[0] = timestamps_1[i];
+                            capture_2[0] = channel_2[i];
+                            capturestamps_2[0] = timestamps_2[i];
+                            for(int i = 1; i < captureLength; i++) {
+                                capture_1[i] = getAudioValue(mic1);
+                                capturestamps_1[i] = t.read_us();
+                                capture_2[i] = getAudioValue(mic2);
+                                capturestamps_2[i] = t.read_us();
+                            }
+                            quit = true;
                         }
-                        quit = true;
+                        if(i < dataLength) {
+                            i++;
+                        } else {
+                            i = 0;
+                        }
                     }
-                    if(i < dataLength) {
-                        i++;
-                    } else {
-                        i = 0;
-                    }
+                
+                
+                    int positionOfMaxVal_1 = FindPeak(1);
+                    int positionOfMaxVal_2 = FindPeak(2);
+                    avgTimedelay =  avgTimedelay + FindTimeDelay(positionOfMaxVal_1, positionOfMaxVal_2); //microseceonds
+                    counter++;
                 }
+                timedelay = avgTimedelay*0.001;
+                DebugPrintDelay(std::cout<<timedelay<<std::endl;);
                 STATE = CALC;
                 break;
 
@@ -314,11 +329,11 @@
             case CALC:
                 DebugPrintState( std::cout << "Nucleo state is CALC: " << std::endl; );
                 //Debug( wait(0.5); );
-
-                int positionOfMaxVal_1 = FindPeak(1);
-                int positionOfMaxVal_2 = FindPeak(2);
+                timedelay = avgTimedelay;
+                //int positionOfMaxVal_1 = FindPeak(1);
+                //int positionOfMaxVal_2 = FindPeak(2);
                 //run functions
-                double timedelay = FindTimeDelay(positionOfMaxVal_1, positionOfMaxVal_2); //microseceonds
+                //double timedelay = FindTimeDelay(positionOfMaxVal_1, positionOfMaxVal_2); //microseceonds
                 if(abs(timedelay) > micDist/calcSoundSpeed(temp, hum)){
                     STATE = CALC_ERROR;
                     break;