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@8:aaf5cde0aa0a, 2016-04-27 (annotated)

Committer:
mikeb
Date:
Wed Apr 27 20:12:55 2016 +0000
Revision:
8:aaf5cde0aa0a
Parent:
0:adae25491b93
Child:
9:d86d73964999
Smoothing Function

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 8:aaf5cde0aa0a 43 float change = 0;
mikeb 8:aaf5cde0aa0a 44
mikeb 8:aaf5cde0aa0a 45 for (int i = 2; i < length - 1; i++) {
mikeb 8:aaf5cde0aa0a 46 change = abs(samples1[i - 2] - samples1[i - 1]);
mikeb 8:aaf5cde0aa0a 47 if (abs(samples1[i] - samples1[i-1]) > change*4.5)
mikeb 8:aaf5cde0aa0a 48 samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
mikeb 8:aaf5cde0aa0a 49 }
mikeb 0:adae25491b93 50
mikeb 0:adae25491b93 51 for (int j = 0; j<peaks; j++) {
mikeb 0:adae25491b93 52 float max = 0;
mikeb 0:adae25491b93 53
mikeb 0:adae25491b93 54 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
mikeb 0:adae25491b93 55 if (max < samples1[i]) {
mikeb 0:adae25491b93 56 max = samples1[i];
mikeb 0:adae25491b93 57 indices1[j] = i;
mikeb 0:adae25491b93 58 }
mikeb 0:adae25491b93 59 }
mikeb 0:adae25491b93 60 if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
mikeb 0:adae25491b93 61 for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
mikeb 0:adae25491b93 62 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 63 }
mikeb 0:adae25491b93 64 }
mikeb 0:adae25491b93 65 else {
mikeb 0:adae25491b93 66 for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
mikeb 0:adae25491b93 67 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 68 }
mikeb 0:adae25491b93 69 }
mikeb 0:adae25491b93 70
mikeb 0:adae25491b93 71 }
mikeb 0:adae25491b93 72 }
mikeb 0:adae25491b93 73
mikeb 0:adae25491b93 74
mikeb 0:adae25491b93 75 float Phase_Finder::est_Phase() {
mikeb 0:adae25491b93 76 float avgDist = 0;
mikeb 0:adae25491b93 77 float ph;
mikeb 0:adae25491b93 78 for (int i = 0; i<peaks - 1; i++){
mikeb 0:adae25491b93 79 avgDist += indices1[i] - ceil(sampleRate/frequency*i);
mikeb 0:adae25491b93 80 }
mikeb 0:adae25491b93 81 avgDist = avgDist / (peaks- 1);
mikeb 0:adae25491b93 82 ph = avgDist / float(sampleRate) * float(frequency) *float(360);
mikeb 0:adae25491b93 83 return ph;
mikeb 0:adae25491b93 84 }
mikeb 0:adae25491b93 85
mikeb 0:adae25491b93 86 float Phase_Finder::estimate(float sampl[], int leng) {
mikeb 0:adae25491b93 87 length = leng;
mikeb 0:adae25491b93 88 peaks = floor(frequency / sampleRate*length);
mikeb 0:adae25491b93 89 est_Max(sampl);
mikeb 0:adae25491b93 90 return est_Phase();
mikeb 0:adae25491b93 91
mikeb 0:adae25491b93 92 }