File content as of revision 14:8865ec38bdc8:

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

/** Phase_Finder.h
*   Computes the phase of a signal using correlation
*   EXAMPLE:
    @code
    //Estimate the phase of a 900Hz input signal.
    //Requires an input cosine reference. Generated in MATLAB and placed into an array not shown. 
    float reference[307] = {...};
    float phase = 0;
    Phase_Finder phase(50000, 900, reference);
    phase - phase.estimate(signal, 251);
* }
* @endcode
*/

class Phase_Finder {

public:
    /** Create a Phase_Finder object
    *
    * @param sampleRate ADC Sample Rate
    * @param frequency Frequency of Interest in Hz
    * @param reference[] Cosine Reference signal used for correlation. Must be input sample length + ceil(sampleRate/Frequency). Must be same frequency as input frequency
    */
    Phase_Finder(int sampleRate, float frequency, float reference[]);
    /** Estimate the phase
    *
    * @param samples[] Input samples
    * @param leng Length of input sample array
    */
    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;
    //Remove random high amplitude distortion
    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;
    }
    //Cross correlation
    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];
        }
    }
    //Find maximum correlation
    float max = 0;
    for (int i = 0; i < 56; i++) {
        if (max < corr[i]) {
            max = corr[i];
            indices1[0] = i;
        }
    }
    //Peak finder method. Inferior to correlation
    /*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;
    //Used for peak finding averaging. Not necessary.
    //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();

}