File content as of revision 11:bdad1acccdad:

#include "FrequencyFinder.h"

using namespace std;

FrequencyFinder::FrequencyFinder(PinName input) : _signal_in(input), _dc_offset(p18) {
    index=0;
    peak=0;
    frequency_final=0;
}

FrequencyFinder::~FrequencyFinder() {
    ticker.detach();
}

float FrequencyFinder::find_frequency() {
    initialize_array();
    ticker.attach_us(this, &FrequencyFinder::get_data,500);

    wait(1);
    ticker.detach();

    copy_data();

    vRealFFT(signal_array,size);

    take_abs();

    int peak = find_peak();

    float frequency_final=((float)peak)/4.1;

    return frequency_final;
}

void FrequencyFinder::get_data() {
    if (index<size) {
        signal_array[index]=_signal_in.read();//-0.5;
        index++;
    }
}

void FrequencyFinder::take_abs() {
//takes the absolute value of the signal
    for (int j=0; j<size; j++) {
        signal_array[j]=abs(signal_array[j]);
        if (j<200)//~48Hz
            signal_array[j]=0;
        else if (j<400)//~97Hz
            signal_array[j]=signal_array[j]*5;
        else if (j<600)//~146Hz
            signal_array[j]=signal_array[j]*6;
        else if (j<900)//~219Hz
            signal_array[j]=signal_array[j]*7;
        else if (j<1200)//292Hz
            signal_array[j]=signal_array[j]*25;
        else
            signal_array[j]=signal_array[j]*30;
    }
}

int FrequencyFinder::find_peak() {
    int i=0;

    for (int j=0; j<size; j++) {
        if ((signal_array[i]-signal_array[i-1])>100 ) {
            while (
                (signal_array[i]<=signal_array[i+1]) &&
                (signal_array[i]<=signal_array[i+2]) &&
                (signal_array[i]<=signal_array[i+3]) &&
                (signal_array[i]<=signal_array[i+4])) {
                i++;
            }
            return FrequencyFinder::find_max(i);
        }
        i++;
    }
    return FrequencyFinder::find_max(i);
}

int FrequencyFinder::find_max(int position) {
    int index1=position-20;
    int index2=position+20;
    float max=signal_array[index1];
    int max_index=0;

    for (int a=index1; a<index2; a++) {
        if (signal_array[a]>max) {
            max=signal_array[a];
            max_index=a;
        }
    }

    return max_index;
}

void FrequencyFinder::initialize_array() {
    index=0;
    for (int w=0; w<size; w++) {
        signal_array[w]=0;
    }
}

void FrequencyFinder::copy_data() {
    for (int i=0; i<size/2; i++) {
        signal_array[i+(size/2)]=signal_array[i];
    }
}