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

Committer:
mikeb
Date:
Thu Apr 28 06:15:54 2016 +0000
Revision:
9:d86d73964999
Parent:
8:aaf5cde0aa0a
Child:
10:cd3f7010da48
Before 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 0:adae25491b93 22 class Phase_Finder {
mikeb 0:adae25491b93 23
mikeb 0:adae25491b93 24 public:
mikeb 0:adae25491b93 25 /** Create a
mikeb 0:adae25491b93 26 *
mikeb 0:adae25491b93 27 * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
mikeb 0:adae25491b93 28 *
mikeb 0:adae25491b93 29 * @note Supported types of sensors:
mikeb 0:adae25491b93 30 */
mikeb 0:adae25491b93 31 Phase_Finder(int sampleRate, float frequency);
mikeb 0:adae25491b93 32 float estimate(float samples[], int leng);
mikeb 0:adae25491b93 33
mikeb 0:adae25491b93 34
mikeb 0:adae25491b93 35 private:
mikeb 0:adae25491b93 36 float est_Phase();
mikeb 0:adae25491b93 37 void est_Max(float samples[]);
mikeb 0:adae25491b93 38 float wavelength;
mikeb 0:adae25491b93 39 float frequency;
mikeb 0:adae25491b93 40 int sampleRate;
mikeb 0:adae25491b93 41 int indices1[PEAKS];
mikeb 0:adae25491b93 42 int length;
mikeb 0:adae25491b93 43 int peaks;
mikeb 0:adae25491b93 44 float phase;
mikeb 0:adae25491b93 45
mikeb 0:adae25491b93 46 };
mikeb 0:adae25491b93 47 Phase_Finder::Phase_Finder(int nsampleRate, float freq) : sampleRate(nsampleRate), frequency(freq)
mikeb 0:adae25491b93 48 {
mikeb 0:adae25491b93 49 //wavelength = (338.4/freq);
mikeb 0:adae25491b93 50 }
mikeb 0:adae25491b93 51
mikeb 0:adae25491b93 52 void Phase_Finder::est_Max(float samples1[]) {
mikeb 8:aaf5cde0aa0a 53 float change = 0;
mikeb 8:aaf5cde0aa0a 54
mikeb 8:aaf5cde0aa0a 55 for (int i = 2; i < length - 1; i++) {
mikeb 8:aaf5cde0aa0a 56 change = abs(samples1[i - 2] - samples1[i - 1]);
mikeb 8:aaf5cde0aa0a 57 if (abs(samples1[i] - samples1[i-1]) > change*4.5)
mikeb 8:aaf5cde0aa0a 58 samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
mikeb 8:aaf5cde0aa0a 59 }
mikeb 0:adae25491b93 60
mikeb 0:adae25491b93 61 for (int j = 0; j<peaks; j++) {
mikeb 0:adae25491b93 62 float max = 0;
mikeb 0:adae25491b93 63
mikeb 0:adae25491b93 64 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
mikeb 0:adae25491b93 65 if (max < samples1[i]) {
mikeb 0:adae25491b93 66 max = samples1[i];
mikeb 0:adae25491b93 67 indices1[j] = i;
mikeb 0:adae25491b93 68 }
mikeb 0:adae25491b93 69 }
mikeb 0:adae25491b93 70 if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
mikeb 0:adae25491b93 71 for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
mikeb 0:adae25491b93 72 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 73 }
mikeb 0:adae25491b93 74 }
mikeb 0:adae25491b93 75 else {
mikeb 0:adae25491b93 76 for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
mikeb 0:adae25491b93 77 samples1[indices1[j] + i] = 0;
mikeb 0:adae25491b93 78 }
mikeb 0:adae25491b93 79 }
mikeb 0:adae25491b93 80
mikeb 0:adae25491b93 81 }
mikeb 0:adae25491b93 82 }
mikeb 0:adae25491b93 83
mikeb 0:adae25491b93 84
mikeb 0:adae25491b93 85 float Phase_Finder::est_Phase() {
mikeb 0:adae25491b93 86 float avgDist = 0;
mikeb 0:adae25491b93 87 float ph;
mikeb 0:adae25491b93 88 for (int i = 0; i<peaks - 1; i++){
mikeb 0:adae25491b93 89 avgDist += indices1[i] - ceil(sampleRate/frequency*i);
mikeb 0:adae25491b93 90 }
mikeb 0:adae25491b93 91 avgDist = avgDist / (peaks- 1);
mikeb 0:adae25491b93 92 ph = avgDist / float(sampleRate) * float(frequency) *float(360);
mikeb 0:adae25491b93 93 return ph;
mikeb 0:adae25491b93 94 }
mikeb 0:adae25491b93 95
mikeb 0:adae25491b93 96 float Phase_Finder::estimate(float sampl[], int leng) {
mikeb 0:adae25491b93 97 length = leng;
mikeb 0:adae25491b93 98 peaks = floor(frequency / sampleRate*length);
mikeb 0:adae25491b93 99 est_Max(sampl);
mikeb 0:adae25491b93 100 return est_Phase();
mikeb 0:adae25491b93 101
mikeb 0:adae25491b93 102 }