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@10:cd3f7010da48, 2016-04-28 (annotated)

Committer:
mikeb
Date:
Thu Apr 28 06:16:10 2016 +0000
Revision:
10:cd3f7010da48
Parent:
9:d86d73964999
Child:
14:8865ec38bdc8
After Corr;

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 9:d86d73964999 6 /** AoA_Est.h
mikeb 9:d86d73964999 7 * Computes the angle of arrival of an audio signal
mikeb 9:d86d73964999 8 * EXAMPLE:
mikeb 9:d86d73964999 9 @code
mikeb 9:d86d73964999 10 //Find the angle of arrival of a 900Hz audio signal using 3 MBEDS equipped with microphones.
mikeb 9:d86d73964999 11 //Requires an array of phases, found using Phase_Finder and communication between MBEDs.
mikeb 9:d86d73964999 12 //This code begins after phases have been found and communicated to master MBED
mikeb 9:d86d73964999 13 //Sensors are located at (0,0), (150,0), and (150,90). Maximum sensors is 10 including master. Can be changed with parameter MAX_SENSORS
mikeb 9:d86d73964999 14 float angle = 0;
mikeb 9:d86d73964999 15 int x[2] = {150, 150); //Distances from master MBED microphone in milimeters. All distances must be for microphones, not MBED units
mikeb 9:d86d73964999 16 int y[2] = {0, 90); //Do not include master MBED, its microphone is assumed to be 0,0.
mikeb 9:d86d73964999 17 AoA_Est AoA(3, x, y, 900);
mikeb 9:d86d73964999 18 angle = AoA.estimate(phases, phases);
mikeb 0:adae25491b93 19 * }
mikeb 0:adae25491b93 20 * @endcode
mikeb 0:adae25491b93 21 */
mikeb 10:cd3f7010da48 22
mikeb 0:adae25491b93 23 class Phase_Finder {
mikeb 0:adae25491b93 24
mikeb 0:adae25491b93 25 public:
mikeb 0:adae25491b93 26 /** Create a
mikeb 0:adae25491b93 27 *
mikeb 0:adae25491b93 28 * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
mikeb 0:adae25491b93 29 *
mikeb 0:adae25491b93 30 * @note Supported types of sensors:
mikeb 0:adae25491b93 31 */
mikeb 10:cd3f7010da48 32 Phase_Finder(int sampleRate, float frequency, float reference[]);
mikeb 0:adae25491b93 33 float estimate(float samples[], int leng);
mikeb 0:adae25491b93 34
mikeb 0:adae25491b93 35
mikeb 0:adae25491b93 36 private:
mikeb 10:cd3f7010da48 37 float reference[SAMPLE_LENGTH+56];
mikeb 0:adae25491b93 38 float est_Phase();
mikeb 0:adae25491b93 39 void est_Max(float samples[]);
mikeb 0:adae25491b93 40 float wavelength;
mikeb 0:adae25491b93 41 float frequency;
mikeb 0:adae25491b93 42 int sampleRate;
mikeb 0:adae25491b93 43 int indices1[PEAKS];
mikeb 0:adae25491b93 44 int length;
mikeb 0:adae25491b93 45 int peaks;
mikeb 0:adae25491b93 46 float phase;
mikeb 0:adae25491b93 47
mikeb 0:adae25491b93 48 };
mikeb 10:cd3f7010da48 49 Phase_Finder::Phase_Finder(int nsampleRate, float freq, float ref[]) : sampleRate(nsampleRate), frequency(freq)
mikeb 0:adae25491b93 50 {
mikeb 10:cd3f7010da48 51 for (int i = 0; i < SAMPLE_LENGTH + 56; i++) {
mikeb 10:cd3f7010da48 52 reference[i] = ref[i];
mikeb 10:cd3f7010da48 53 }
mikeb 0:adae25491b93 54 }
mikeb 0:adae25491b93 55
mikeb 0:adae25491b93 56 void Phase_Finder::est_Max(float samples1[]) {
mikeb 8:aaf5cde0aa0a 57 float change = 0;
mikeb 8:aaf5cde0aa0a 58
mikeb 8:aaf5cde0aa0a 59 for (int i = 2; i < length - 1; i++) {
mikeb 8:aaf5cde0aa0a 60 change = abs(samples1[i - 2] - samples1[i - 1]);
mikeb 8:aaf5cde0aa0a 61 if (abs(samples1[i] - samples1[i-1]) > change*4.5)
mikeb 8:aaf5cde0aa0a 62 samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
mikeb 8:aaf5cde0aa0a 63 }
mikeb 10:cd3f7010da48 64 float corr[56] = {};
mikeb 10:cd3f7010da48 65 for (int j = 0; j < 56; j++) {
mikeb 10:cd3f7010da48 66 for (int i = 0; i <= length; i++) {
mikeb 10:cd3f7010da48 67 corr[j] += reference[i-j+55] * samples1[i];
mikeb 10:cd3f7010da48 68 }
mikeb 10:cd3f7010da48 69 }
mikeb 10:cd3f7010da48 70 float max = 0;
mikeb 10:cd3f7010da48 71 for (int i = 0; i < 56; i++) {
mikeb 10:cd3f7010da48 72 if (max < corr[i]) {
mikeb 10:cd3f7010da48 73 max = corr[i];
mikeb 10:cd3f7010da48 74 indices1[0] = i;
mikeb 10:cd3f7010da48 75 }
mikeb 10:cd3f7010da48 76 }
mikeb 10:cd3f7010da48 77
mikeb 10:cd3f7010da48 78 /*for (int j = 0; j<peaks; j++) {
mikeb 0:adae25491b93 79 float max = 0;
mikeb 0:adae25491b93 80
mikeb 0:adae25491b93 81 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
mikeb 0:adae25491b93 82 if (max < samples1[i]) {
mikeb 0:adae25491b93 83 max = samples1[i];
mikeb 0:adae25491b93 84 indices1[j] = i;
mikeb 0:adae25491b93 85 }
mikeb 0:adae25491b93 86 }
mikeb 10:cd3f7010da48 87 if (indices1[j] - round(sampleRate / frequency)/2 >= 0) {
mikeb 10:cd3f7010da48 88 for (int i = -round(sampleRate / frequency)/2; i< round(sampleRate/frequency) / 2; i++) {
mikeb 0:adae25491b93 89 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 90 }
mikeb 0:adae25491b93 91 }
mikeb 0:adae25491b93 92 else {
mikeb 10:cd3f7010da48 93 for (int i = 0; i< indices1[j] + round(sampleRate / frequency) / 2; i++) {
mikeb 0:adae25491b93 94 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 95 }
mikeb 0:adae25491b93 96 }
mikeb 0:adae25491b93 97
mikeb 10:cd3f7010da48 98 }*/
mikeb 0:adae25491b93 99 }
mikeb 0:adae25491b93 100
mikeb 0:adae25491b93 101
mikeb 0:adae25491b93 102 float Phase_Finder::est_Phase() {
mikeb 10:cd3f7010da48 103 //float avgDist = 0;
mikeb 0:adae25491b93 104 float ph;
mikeb 10:cd3f7010da48 105 //for (int i = 0; i<peaks - 1; i++){
mikeb 10:cd3f7010da48 106 // avgDist += indices1[i] - round(sampleRate/frequency*i);
mikeb 10:cd3f7010da48 107 //}
mikeb 10:cd3f7010da48 108 //avgDist = avgDist / (peaks- 1);
mikeb 10:cd3f7010da48 109 ph = indices1[0] / float(sampleRate) * float(frequency) *float(360);
mikeb 0:adae25491b93 110 return ph;
mikeb 0:adae25491b93 111 }
mikeb 0:adae25491b93 112
mikeb 0:adae25491b93 113 float Phase_Finder::estimate(float sampl[], int leng) {
mikeb 0:adae25491b93 114 length = leng;
mikeb 0:adae25491b93 115 peaks = floor(frequency / sampleRate*length);
mikeb 0:adae25491b93 116 est_Max(sampl);
mikeb 0:adae25491b93 117 return est_Phase();
mikeb 0:adae25491b93 118
mikeb 0:adae25491b93 119 }