Diff: Phase_Finder.h

Revision:

10:cd3f7010da48

Parent:

9:d86d73964999

Child:

14:8865ec38bdc8

diff -r d86d73964999 -r cd3f7010da48 Phase_Finder.h
--- a/Phase_Finder.h	Thu Apr 28 06:15:54 2016 +0000
+++ b/Phase_Finder.h	Thu Apr 28 06:16:10 2016 +0000
@@ -19,6 +19,7 @@
 * }
 * @endcode
 */
+
 class Phase_Finder {
 
 public:
@@ -28,11 +29,12 @@
     *
     * @note Supported types of sensors:
     */
-    Phase_Finder(int sampleRate, float frequency);
+    Phase_Finder(int sampleRate, float frequency, float reference[]);
     float estimate(float samples[], int leng);
 
 
 private:
+    float reference[SAMPLE_LENGTH+56];
     float est_Phase();
     void est_Max(float samples[]);
     float wavelength;
@@ -44,9 +46,11 @@
     float phase;
 
 };
-Phase_Finder::Phase_Finder(int nsampleRate, float freq) : sampleRate(nsampleRate), frequency(freq)
+Phase_Finder::Phase_Finder(int nsampleRate, float freq, float ref[]) : sampleRate(nsampleRate), frequency(freq)
 {
-    //wavelength = (338.4/freq);
+    for (int i = 0; i < SAMPLE_LENGTH + 56; i++) {
+        reference[i] = ref[i];
+    }
 }
 
 void Phase_Finder::est_Max(float samples1[]) {
@@ -57,8 +61,21 @@
         if (abs(samples1[i] - samples1[i-1]) > change*4.5)
             samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
     }
-    
-    for (int j = 0; j<peaks; j++) {
+    float corr[56] = {};
+    for (int j = 0; j < 56; j++) {
+        for (int i = 0; i <= length; i++) {
+            corr[j] += reference[i-j+55] * samples1[i];
+        }
+    }
+    float max = 0;
+    for (int i = 0; i < 56; i++) {
+        if (max < corr[i]) {
+            max = corr[i];
+            indices1[0] = i;
+        }
+    }
+
+    /*for (int j = 0; j<peaks; j++) {
         float max = 0;
         
         for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
@@ -67,29 +84,29 @@
                 indices1[j] = i;
             }
         }
-        if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
-            for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
+        if (indices1[j] - round(sampleRate / frequency)/2 >= 0) {
+            for (int i = -round(sampleRate / frequency)/2; i< round(sampleRate/frequency) / 2; i++) {
                 samples1[indices1[j] + i] = 0;
             }
         }
         else {
-            for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
+            for (int i = 0; i< indices1[j] + round(sampleRate / frequency) / 2; i++) {
                 samples1[indices1[j] + i] = 0;
             }
         }
         
-    }
+    }*/
 }
 
 
 float Phase_Finder::est_Phase() {
-    float avgDist = 0;
+    //float avgDist = 0;
     float ph;
-    for (int i = 0; i<peaks - 1; i++){
-        avgDist += indices1[i] - ceil(sampleRate/frequency*i);
-    }
-    avgDist = avgDist / (peaks- 1);
-        ph = avgDist / float(sampleRate) * float(frequency) *float(360);
+    //for (int i = 0; i<peaks - 1; i++){
+    //  avgDist += indices1[i] - round(sampleRate/frequency*i);
+    //}
+    //avgDist = avgDist / (peaks- 1);
+        ph = indices1[0] / float(sampleRate) * float(frequency) *float(360);
         return ph;
 }