File content as of revision 10:cd3f7010da48:

#include <mbed.h>
using namespace std;
const int SAMPLE_LENGTH = 251;
const int PEAKS = 4;

/** AoA_Est.h
*   Computes the angle of arrival of an audio signal
*   EXAMPLE:
    @code
    //Find the angle of arrival of a 900Hz audio signal using 3 MBEDS equipped with microphones. 
    //Requires an array of phases, found using Phase_Finder and communication between MBEDs. 
    //This code begins after phases have been found and communicated to master MBED
    //Sensors are located at (0,0), (150,0), and (150,90). Maximum sensors is 10 including master. Can be changed with parameter MAX_SENSORS
    float angle = 0;
    int x[2] = {150, 150); //Distances from master MBED microphone in milimeters. All distances must be for microphones, not MBED units
    int y[2] = {0, 90);  //Do not include master MBED, its microphone is assumed to be 0,0.
    AoA_Est AoA(3, x, y, 900);
    angle = AoA.estimate(phases, phases);
* }
* @endcode
*/

class Phase_Finder {

public:
    /** Create a
    *
    * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
    *
    * @note Supported types of sensors:
    */
    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;
    float frequency;
    int sampleRate;
    int indices1[PEAKS];
    int length;
    int peaks;
    float phase;

};
Phase_Finder::Phase_Finder(int nsampleRate, float freq, float ref[]) : sampleRate(nsampleRate), frequency(freq)
{
    for (int i = 0; i < SAMPLE_LENGTH + 56; i++) {
        reference[i] = ref[i];
    }
}

void Phase_Finder::est_Max(float samples1[]) {
    float change = 0;
    
    for (int i = 2; i < length - 1; i++) {
        change = abs(samples1[i - 2] - samples1[i - 1]);
        if (abs(samples1[i] - samples1[i-1]) > change*4.5)
            samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
    }
    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++) {
            if (max < samples1[i]) {
                max = samples1[i];
                indices1[j] = 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] + round(sampleRate / frequency) / 2; i++) {
                samples1[indices1[j] + i] = 0;
            }
        }
        
    }*/
}


float Phase_Finder::est_Phase() {
    //float avgDist = 0;
    float ph;
    //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;
}

float Phase_Finder::estimate(float  sampl[], int leng) {
    length = leng;
    peaks = floor(frequency / sampleRate*length);
    est_Max(sampl);
        return est_Phase();

}