m b
/
AoA_estimator
Part of a program that estimates the direction of arrival of a signal
Revision 10:cd3f7010da48, committed 2016-04-28
- Comitter:
- mikeb
- Date:
- Thu Apr 28 06:16:10 2016 +0000
- Parent:
- 9:d86d73964999
- Child:
- 11:8574bb5b92b0
- Commit message:
- After Corr;
Changed in this revision
Phase_Finder.h | Show annotated file Show diff for this revision Revisions of this file |
--- 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; }