FrequencyFinder.cpp

00001 #include "FrequencyFinder.h"
00002 
00003 using namespace std;
00004 
00005 FrequencyFinder::FrequencyFinder(PinName input) : _signal_in(input), _dc_offset(p18) {
00006     index=0;
00007     peak=0;
00008     frequency_final=0;
00009 }
00010 
00011 FrequencyFinder::~FrequencyFinder() {
00012     ticker.detach();
00013 }
00014 
00015 float FrequencyFinder::find_frequency() {
00016     initialize_array();
00017     ticker.attach_us(this, &FrequencyFinder::get_data,500);
00018 
00019     wait(1);
00020     ticker.detach();
00021 
00022     copy_data();
00023 
00024     vRealFFT(signal_array,size);
00025 
00026     take_abs();
00027 
00028     int peak = find_peak();
00029 
00030     float frequency_final=((float)peak)/4.1;
00031 
00032     return frequency_final;
00033 }
00034 
00035 void FrequencyFinder::get_data() {
00036     if (index<size) {
00037         signal_array[index]=_signal_in.read();//-0.5;
00038         index++;
00039     }
00040 }
00041 
00042 void FrequencyFinder::take_abs() {
00043 //takes the absolute value of the signal
00044     for (int j=0; j<size; j++) {
00045         signal_array[j]=abs(signal_array[j]);
00046         if (j<200)//~48Hz
00047             signal_array[j]=0;
00048         else if (j<400)//~97Hz
00049             signal_array[j]=signal_array[j]*3;
00050         else if (j<550)//~
00051             signal_array[j]=signal_array[j]*6;
00052         else if (j<900)//~219Hz
00053             signal_array[j]=signal_array[j]*10;
00054         else if (j<1200)//292Hz
00055             signal_array[j]=signal_array[j]*12;
00056         else
00057             signal_array[j]=signal_array[j]*30;
00058     }
00059 }
00060 
00061 int FrequencyFinder::find_peak() {
00062     int i=0;
00063 
00064     for (int j=0; j<size; j++) {
00065         if ((signal_array[i]-signal_array[i-1])>100 ) {
00066             while (
00067                 (signal_array[i]<=signal_array[i+1]) &&
00068                 (signal_array[i]<=signal_array[i+2]) &&
00069                 (signal_array[i]<=signal_array[i+3]) &&
00070                 (signal_array[i]<=signal_array[i+4])) {
00071                 i++;
00072             }
00073             return FrequencyFinder::find_max(i);
00074         }
00075         i++;
00076     }
00077     return FrequencyFinder::find_max(i);
00078 }
00079 
00080 int FrequencyFinder::find_max(int position) {
00081     int index1=position-20;
00082     int index2=position+20;
00083     float max=signal_array[index1];
00084     int max_index=0;
00085 
00086     for (int a=index1; a<index2; a++) {
00087         if (signal_array[a]>max) {
00088             max=signal_array[a];
00089             max_index=a;
00090         }
00091     }
00092 
00093     return max_index;
00094 }
00095 
00096 void FrequencyFinder::initialize_array() {
00097     index=0;
00098     for (int w=0; w<size; w++) {
00099         signal_array[w]=0;
00100     }
00101 }
00102 
00103 void FrequencyFinder::copy_data() {
00104     for (int i=0; i<size/2; i++) {
00105         signal_array[i+(size/2)]=signal_array[i];
00106     }
00107 }