File content as of revision 15:29805fab7655:

#pragma once
#include "mbed.h"
#include <cmath>
const int MAX_SENSORS = 10; //Maxmimum number of sensors
/** 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 AoA_Est {

public:
    /** Create an Angle of Arrival Estimator Object
         *
         * @param numOfSensors Number of sensors including the master
         * @param xPassed[] X coordinates of every sensor EXCEPT the master. Master is assumed (0,0). 
         * @param yPassed[] Y coordinates of every sensor EXCEPT the master
         * @param freq frequency of interest in Hz
         * @note Maximum number of supported sensors is 10
         */
    AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq);
    /** Estimate the angle to the sound source
         *
         * @param phases[] Array of phases, first phase is the master. Must correspond to order of X and Y coordinates. All sensors must be within 1/2 wavelength of eachother. Will not work otherwise. 
         * @param amp[] Amplitudes. Not currently used. Repeat the phase array here to prevent errors.
         * @param yPassed[] Y coordinates of every sensor EXCEPT the master
         * @return Angle relative to X axis
         */
    float estimate(float phases[], float amp[]);
    /** Not Used
    */
    bool calibrate();
    /** Confidence level of the output
    *
    * @return Float corresponding to the number of angle determinations used in the average
    */
    int confidence;

private:
    float estimate_Theoretical(float phases[], float amp[]); 
    float estimate_Calibrated(float phases[], float amp[]); //Unused
    void comparative_Phases(float phas[]); //Returns the phases relative to the master
    float angle_Resolver();
    float distanceFinder(float phase);
    int sensors;
    int ITERATIONS;
    int x[MAX_SENSORS];
    int y[MAX_SENSORS];
    float phases[MAX_SENSORS - 1];
    float sensorSep[MAX_SENSORS];
    float sensorAngles[MAX_SENSORS];
    float amplitudes[MAX_SENSORS];
    int z[2];
    float ambigAngles[2][MAX_SENSORS];
    float wavelength;

};
AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors)
{
    int j = sensors - 1;
    while (j > 0){
        ITERATIONS += j;
        j--;        
    }
    wavelength = (338.4 / freq)*1000;
    for (short i = 0; i < sensors-1; i++) {
        x[i] = xPassed[i];
        y[i] = yPassed[i];
        sensorSep[i] = sqrt(float(xPassed[i] * x[i]) + float(yPassed[i] * y[i]));
        sensorAngles[i] = atan2(float(yPassed[i]), float(xPassed[i]))*180/3.1415926535;
    }
}

float AoA_Est::distanceFinder(float phase) {
    return phase / 360 * wavelength;
}

float AoA_Est::estimate_Theoretical(float phases[], float amp[]) {
    float distance = 0;
    float angle = 0;

    for (int i = 0; i < sensors-1; i++) {
        distance = distanceFinder(phases[i]);
        distance = (distance > sensorSep[i]) ? distance - 338400/50000 : distance;
        angle = acos(distance / sensorSep[i])*180/3.1415923535;
        ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/-
        ambigAngles[1][i] = sensorAngles[i] + angle;

        ambigAngles[0][i] = (ambigAngles[0][i] < 0) ? ambigAngles[0][i] + 360 : ambigAngles[0][i];
        ambigAngles[1][i] = (ambigAngles[1][i] < 0) ? ambigAngles[1][i] + 360 : ambigAngles[1][i];
    }
    
    float phas_diff = 0;
    float relative_angle = 0;
    float relative_dist = 0;
    //Accuracy improvement
    /*for (int i = 1; i < sensors - 1; i++) {   //for(int i = 1; i<ITERATIONS - sensors+1; i++)
        while (i < sensors - 1) {
            phas_diff = phases[i - 1] - phases[i];
            if (abs(phas_diff) > 180) {
                phas_diff = (phas_diff < 0) ? phas_diff + 360 : phas_diff - 360;
            }
            distance = distanceFinder(phas_diff);
            relative_angle = atan2(float(y[i - 1] - y[i]), float((x[i - 1] - x[i]))) * 180 / 3.1415926535;
            relative_dist = sqrt(float((x[i - 1] - x[i]) *(x[i - 1] - x[i])) + float((y[i - 1] - y[i]) *(y[i - 1] - y[i])));
            angle = acos(distance / relative_dist) * 180 / 3.1415923535;
            ambigAngles[0][sensors - 2 + i] = relative_angle - angle;
            ambigAngles[1][sensors - 2 + i] = relative_angle + angle;

            ambigAngles[0][sensors - 2 + i] = (ambigAngles[0][sensors - 2 + i] < 0) ? ambigAngles[0][sensors - 2 + i] + 360 : ambigAngles[0][sensors - 2 + i];
            ambigAngles[1][sensors - 2 + i] = (ambigAngles[1][sensors - 2 + i] < 0) ? ambigAngles[1][sensors - 2 + i] + 360 : ambigAngles[1][sensors - 2 + i];
            i++;
        }
    }*/
    angle = angle_Resolver();
    return angle;
}

float AoA_Est::angle_Resolver() {
    float angle = ambigAngles[0][0];
    float avg = angle;
    bool flag = false;
    confidence = 0;
    for (short i = 1; i <= sensors-1; i++) {
        if (abs(angle - ambigAngles[0][i]) < abs(angle-ambigAngles[1][i]) && abs(angle-ambigAngles[0][i]) < 40) {
            angle = ambigAngles[0][i];
            avg += ambigAngles[0][i];
            confidence++;
        }
        else if (abs(angle - ambigAngles[1][i]) < abs(angle-ambigAngles[0][i]) && abs(angle - ambigAngles[1][i]) < 40) {
            angle = ambigAngles[1][i];
            avg += ambigAngles[1][i];
            confidence++;
        }
        else if (i == 1 && flag == false) {
            angle = ambigAngles[1][0];
            avg = angle;
            i = 0;
            flag = true;
        }

    }

    return avg / (confidence+1);//change when compute the other way
}

void AoA_Est :: comparative_Phases(float phas[]) {
    for (int i = 0; i < sensors - 1; i++) {
        phases[i] = phas[0] - phas[i + 1];
        if (abs(phases[i]) > 180) {
            phases[i] = (phases[i] < 0) ? phases[i] + 360 : phases[i] - 360;
        }

    }

}

float AoA_Est::estimate(float phas[], float amplitudes[]) {
    float angle;
    comparative_Phases(phas);
    angle = estimate_Theoretical(phases, amplitudes);

    return angle;

}