m b / Mbed 2 deprecated AoA_estimator

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed dsp

Phase_Finder.h@0:adae25491b93, 2016-04-18 (annotated)

Committer:
mikeb
Date:
Mon Apr 18 01:14:36 2016 +0000
Revision:
0:adae25491b93
Child:
8:aaf5cde0aa0a
1st Draft. Some accuracy errors when close to sensor axis. ; Needs a filter and a method to process the 2 sensors. ; Filter is a must.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikeb 0:adae25491b93 1 #include <mbed.h>
mikeb 0:adae25491b93 2 using namespace std;
mikeb 0:adae25491b93 3 const int SAMPLE_LENGTH = 251;
mikeb 0:adae25491b93 4 const int PEAKS = 4;
mikeb 0:adae25491b93 5
mikeb 0:adae25491b93 6 /** A
mikeb 0:adae25491b93 7 * }
mikeb 0:adae25491b93 8 * }
mikeb 0:adae25491b93 9 * @endcode
mikeb 0:adae25491b93 10 */
mikeb 0:adae25491b93 11
mikeb 0:adae25491b93 12 class Phase_Finder {
mikeb 0:adae25491b93 13
mikeb 0:adae25491b93 14 public:
mikeb 0:adae25491b93 15 /** Create a
mikeb 0:adae25491b93 16 *
mikeb 0:adae25491b93 17 * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
mikeb 0:adae25491b93 18 *
mikeb 0:adae25491b93 19 * @note Supported types of sensors:
mikeb 0:adae25491b93 20 */
mikeb 0:adae25491b93 21 Phase_Finder(int sampleRate, float frequency);
mikeb 0:adae25491b93 22 float estimate(float samples[], int leng);
mikeb 0:adae25491b93 23
mikeb 0:adae25491b93 24
mikeb 0:adae25491b93 25 private:
mikeb 0:adae25491b93 26 float est_Phase();
mikeb 0:adae25491b93 27 void est_Max(float samples[]);
mikeb 0:adae25491b93 28 float wavelength;
mikeb 0:adae25491b93 29 float frequency;
mikeb 0:adae25491b93 30 int sampleRate;
mikeb 0:adae25491b93 31 int indices1[PEAKS];
mikeb 0:adae25491b93 32 int length;
mikeb 0:adae25491b93 33 int peaks;
mikeb 0:adae25491b93 34 float phase;
mikeb 0:adae25491b93 35
mikeb 0:adae25491b93 36 };
mikeb 0:adae25491b93 37 Phase_Finder::Phase_Finder(int nsampleRate, float freq) : sampleRate(nsampleRate), frequency(freq)
mikeb 0:adae25491b93 38 {
mikeb 0:adae25491b93 39 //wavelength = (338.4/freq);
mikeb 0:adae25491b93 40 }
mikeb 0:adae25491b93 41
mikeb 0:adae25491b93 42 void Phase_Finder::est_Max(float samples1[]) {
mikeb 0:adae25491b93 43
mikeb 0:adae25491b93 44
mikeb 0:adae25491b93 45 for (int j = 0; j<peaks; j++) {
mikeb 0:adae25491b93 46 float max = 0;
mikeb 0:adae25491b93 47
mikeb 0:adae25491b93 48 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
mikeb 0:adae25491b93 49 if (max < samples1[i]) {
mikeb 0:adae25491b93 50 max = samples1[i];
mikeb 0:adae25491b93 51 indices1[j] = i;
mikeb 0:adae25491b93 52 }
mikeb 0:adae25491b93 53 }
mikeb 0:adae25491b93 54 if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
mikeb 0:adae25491b93 55 for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
mikeb 0:adae25491b93 56 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 57 }
mikeb 0:adae25491b93 58 }
mikeb 0:adae25491b93 59 else {
mikeb 0:adae25491b93 60 for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
mikeb 0:adae25491b93 61 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 62 }
mikeb 0:adae25491b93 63 }
mikeb 0:adae25491b93 64
mikeb 0:adae25491b93 65 }
mikeb 0:adae25491b93 66 }
mikeb 0:adae25491b93 67
mikeb 0:adae25491b93 68
mikeb 0:adae25491b93 69 float Phase_Finder::est_Phase() {
mikeb 0:adae25491b93 70 float avgDist = 0;
mikeb 0:adae25491b93 71 float ph;
mikeb 0:adae25491b93 72 for (int i = 0; i<peaks - 1; i++){
mikeb 0:adae25491b93 73 avgDist += indices1[i] - ceil(sampleRate/frequency*i);
mikeb 0:adae25491b93 74 }
mikeb 0:adae25491b93 75 avgDist = avgDist / (peaks- 1);
mikeb 0:adae25491b93 76 ph = avgDist / float(sampleRate) * float(frequency) *float(360);
mikeb 0:adae25491b93 77 return ph;
mikeb 0:adae25491b93 78 }
mikeb 0:adae25491b93 79
mikeb 0:adae25491b93 80 float Phase_Finder::estimate(float sampl[], int leng) {
mikeb 0:adae25491b93 81 length = leng;
mikeb 0:adae25491b93 82 peaks = floor(frequency / sampleRate*length);
mikeb 0:adae25491b93 83 est_Max(sampl);
mikeb 0:adae25491b93 84 return est_Phase();
mikeb 0:adae25491b93 85
mikeb 0:adae25491b93 86 }