main2.cpp

00001 #include "mbed.h"
00002 #include <algorithm>
00003 #include "asyncADC.h"
00004 #include "arm_math.h"
00005 #include "arm_common_tables.h"
00006 #include "moppy.h"
00007 
00008 #define AUDIO_PIN       (A0)
00009  
00010 Serial pc(USBTX, USBRX, 115200);
00011 DigitalOut red(LED_RED), green(LED_GREEN), blue(LED_BLUE);
00012 Timer timer;
00013 InterruptIn filterBtn(SW2);
00014 bool useFilter = false;
00015 
00016 #define LED_ON      (0)
00017 #define LED_OFF     (1)
00018 
00019 #define FFT_SIZE 1024
00020 #include "fft.h"
00021 
00022 #define TARGET_SAMPLE_RATE_HZ  19200 // Sample rate in Hertz
00023 float achievedSampleRate;
00024 
00025 #define BIN_TO_FREQ(bin)  ((bin*(long)achievedSampleRate)/FFT_SIZE)
00026 
00027 #define PC_BUFFER_SIZE (FFT_SIZE+30)
00028 
00029 //BUFFERS
00030 
00031 uint16_t adcBuffer[FFT_SIZE*2];
00032 int spectrumBufferIndex = 0, spectrumBufferLength = 0;
00033 char spectrumBuffer[PC_BUFFER_SIZE];
00034 float fftBuffer[FFT_SIZE*2];
00035 float magnitudes[FFT_SIZE];
00036 
00037 // INTERRUPTS
00038 
00039 volatile int sectionCounter = 0;
00040 volatile bool sectionFilled = false, toggleFilter;
00041 volatile uint32_t sectionStart, sectionEnd;
00042 
00043 void callback(uint32_t start, uint32_t end) {
00044     sectionFilled = true;
00045     sectionCounter++;
00046     sectionStart = start;
00047     sectionEnd = end;
00048 }
00049 
00050 void press() {
00051     toggleFilter=true;
00052 }
00053 
00054 // UTILS
00055 
00056 uint32_t ceilToPowerOf2(uint32_t i) {
00057     i--;
00058     i|=(i>>1);
00059     i|=(i>>2);
00060     i|=(i>>4);
00061     i|=(i>>8);
00062     i|=(i>>16);
00063     return i+1;
00064 }
00065 
00066 void trySwp(int16_t *indices, float *magnitudes, int a, int b) {
00067     if(magnitudes[indices[a]]>magnitudes[indices[b]]) {
00068         int x = indices[a];
00069         indices[a] = indices[b];
00070         indices[b] = x;
00071     }
00072 }
00073 
00074 void sort(int16_t *indices, float *magnitudes, int size) {
00075     indices[0] = 0;
00076     for(int i=1; i<size; i++) {
00077         indices[i]=i;
00078         for(int j=i; j-->0;) {
00079             trySwp(indices, magnitudes, j, j+1);
00080         }
00081     }
00082 }
00083 
00084 bool magComparator(int const & a, int const & b) {
00085     return magnitudes[a] > magnitudes[b];
00086 }
00087 
00088 // POST PROCESSING
00089 
00090 float mqe(float* fft) {
00091     float t0_real, t0_imag, t1_real, t1_imag; //temp values
00092     
00093     t0_real = fft[0]-fft[4];
00094     t0_imag = fft[1]-fft[5];
00095     
00096     t1_real = fft[2]*2.0f - (fft[0] + fft[4]);
00097     t1_imag = fft[3]*2.0f - (fft[1] + fft[5]);
00098     
00099     return (t0_real*t1_real + t0_imag*t1_imag)/(t1_real*t1_real + t1_imag*t1_imag);
00100 }
00101 
00102 void modifiedQuadraticEstimator() {
00103     //Post process
00104     float prev = mqe(&fftBuffer[2]);
00105     for(int i=4; i<FFT_SIZE/2; i+=2) {
00106         //Eric Jacobsen's Modified Quadratic Estimator
00107         float curr = mqe(&fftBuffer[i]);
00108         fftBuffer[i-2]+=prev;
00109         prev = curr;
00110     }
00111     fftBuffer[FFT_SIZE/2]+=prev;
00112 }
00113 
00114 float getMag(int index) {
00115     return (index<1 && index>=FFT_SIZE/2) ? 0.0f : magnitudes[index];
00116 }
00117 
00118 #define FILTER_CUTOFF       (10000.0f)
00119 void localMaximaRad10() {
00120     float p10, p9, p8, p7, p6, p5, p4, p3, p2, p1, c, n1, n2, n3, n4, n5, n6, n7, n8, n9, n10;
00121     c = getMag(1);
00122     n1 = getMag(2);
00123     n2 = getMag(3);
00124     n3 = getMag(4);
00125     n4 = getMag(5);
00126     n5 = getMag(6);
00127     n6 = getMag(7);
00128     n7 = getMag(8);
00129     n8 = getMag(9);
00130     n9 = getMag(10);
00131     n10 = getMag(11);
00132     
00133     magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00134     p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(12);
00135     magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00136     p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(13);
00137     magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00138     p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(14);
00139     magnitudes[4] = (c>=FILTER_CUTOFF && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00140     p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(15);
00141     magnitudes[5] = (c>=FILTER_CUTOFF && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00142     p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(16);
00143     magnitudes[6] = (c>=FILTER_CUTOFF && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00144     p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(17);
00145     magnitudes[7] = (c>=FILTER_CUTOFF && c>p6 && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00146     p7=p6; p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(18);
00147     magnitudes[8] = (c>=FILTER_CUTOFF && c>p7 && c>p6 && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00148     p8=p7; p7=p6; p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(19);
00149     magnitudes[9] = (c>=FILTER_CUTOFF && c>p8 && c>p7 && c>p6 && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00150     p9=p8; p8=p7; p7=p6; p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(20);
00151     magnitudes[10]= (c>=FILTER_CUTOFF && c>p9 && c>p8 && c>p7 && c>p6 && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00152     
00153     for(int i=11; i<FFT_SIZE/2; i++) {
00154         p10=p9; p9=p8; p8=p7; p7=p6; p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=n7; n7=n8; n8=n9; n9=n10; n10=getMag(i+10);
00155         magnitudes[i] = (c>=FILTER_CUTOFF && c>p10 && c>p9 && c>p8 && c>p7 && c>p6 && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6 && c>=n7 && c>=n8 && c>=n9 && c>=n10) ? sqrt(c) : 0;
00156     }
00157 }
00158 void localMaximaRad6() {
00159     float p6, p5, p4, p3, p2, p1, c, n1, n2, n3, n4, n5, n6;
00160     c = getMag(1);
00161     n1 = getMag(2);
00162     n2 = getMag(3);
00163     n3 = getMag(4);
00164     n4 = getMag(5);
00165     n5 = getMag(6);
00166     n6 = getMag(7);
00167     
00168     magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00169     p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(5);
00170     magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00171     p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(6);
00172     magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00173     p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(7);
00174     magnitudes[4] = (c>=FILTER_CUTOFF && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00175     p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(8);
00176     magnitudes[5] = (c>=FILTER_CUTOFF && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00177     p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(9);
00178     magnitudes[6] = (c>=FILTER_CUTOFF && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00179     
00180     for(int i=7; i<FFT_SIZE/2; i++) {
00181         p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(i+6);
00182         magnitudes[i] = (c>=FILTER_CUTOFF && c>p6 && c>p5 && c>p4 && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
00183     }
00184 }
00185 void localMaximaRad3() {
00186     float p3, p2, p1, c, n1, n2, n3;
00187     c = getMag(1);
00188     n1 = getMag(2);
00189     n2 = getMag(3);
00190     n3 = getMag(4);
00191     
00192     magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
00193     p1=c; c=n1; n1=n2; n2=n3; n3=getMag(5);
00194     magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
00195     p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(6);
00196     magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
00197     
00198     for(int i=4; i<FFT_SIZE/2; i++) {
00199         p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(i+3);
00200         magnitudes[i] = (c>=FILTER_CUTOFF && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
00201     }
00202 }
00203 
00204 // MAIN
00205 
00206 int main()
00207 {
00208     filterBtn.fall(press);
00209     red   = LED_OFF;
00210     green = LED_OFF;
00211     blue  = LED_OFF;
00212     
00213     Moppy moppy(D1, D0, 38400); // tx, rx, baud
00214     
00215     achievedSampleRate = approximateFrequency((float)TARGET_SAMPLE_RATE_HZ);
00216     switch(asyncAnalogToCircularBuffer(AUDIO_PIN, adcBuffer, 2*FFT_SIZE, TARGET_SAMPLE_RATE_HZ, callback)) {
00217         case E_ASYNC_ADC_ACTIVE:
00218             error("AsyncADC already in use");
00219         case E_DMA_IN_USE:
00220             error("DMA already in use");
00221         case E_INVALID_BUFFER_SIZE:
00222             error("Invalid destination buffer size");
00223     }
00224     
00225     //fixed header
00226     spectrumBuffer[0]=255;
00227     spectrumBuffer[1]=(FFT_SIZE/2-1)/255;
00228     spectrumBuffer[2]=(FFT_SIZE/2-1)%255;
00229     
00230     timer.start();
00231     
00232     while(1) {
00233         if(toggleFilter) {
00234             useFilter = !useFilter;
00235             toggleFilter = false;
00236         }
00237         if(sectionFilled) {
00238             if(useFilter)
00239                 green = LED_ON;
00240             else
00241                 blue = LED_ON;
00242             
00243             bool buildSpectrumBuffer = false;
00244             
00245             if(spectrumBufferIndex==spectrumBufferLength) {
00246                 buildSpectrumBuffer = true;
00247                 spectrumBufferIndex = 0;
00248             }
00249             
00250             timer.reset();
00251             
00252             //convert to floating point
00253             for(int i=sectionStart, j=0; i<=sectionEnd; i++, j+=2) {
00254                 fftBuffer[j] = (float)adcBuffer[i];
00255                 fftBuffer[j+1] = 0.0f;
00256             }
00257                 
00258             // Calculate FFT if a full sample is available.
00259             // Run FFT on sample data.
00260             arm_cfft_f32(FFT_BUFFER, fftBuffer, 0, 1);
00261             // Calculate magnitude of complex numbers output by the FFT.
00262             
00263             //Convert FFT complex vectors to magnitudes
00264             arm_cmplx_mag_f32(fftBuffer, magnitudes, FFT_SIZE);
00265             
00266             //Post Process / Filtering
00267             if(useFilter) {
00268                 localMaximaRad10();
00269             } else {
00270                 for(int i=FFT_SIZE/2; i-->0;) {
00271                     magnitudes[i]=magnitudes[i]<FILTER_CUTOFF ? 0.0f : sqrt(magnitudes[i]); //trivial amplitude filtering and normalise
00272                 }
00273             }
00274             
00275             if(buildSpectrumBuffer) {
00276                 spectrumBufferLength = 3;
00277                 //data
00278                 for(int i=1; i<(FFT_SIZE/2); i++) {
00279                     int m = ((int)magnitudes[i]+2)>>2;
00280                     spectrumBuffer[spectrumBufferLength++] = m>=254?254:m;
00281                 }
00282             }
00283             
00284             for(int i=moppy.getFloppyDrives(); i-->0;) {
00285                 int peak = 1;
00286                 for(int j=FFT_SIZE/2; j-->2;) {
00287                     if(magnitudes[j]>magnitudes[peak])
00288                         peak=j;
00289                 }
00290                 if(magnitudes[peak]<30.0f) {
00291                     moppy.setFrequency(i, 0);
00292                     while(i-->0)
00293                         moppy.setFrequency(i, 0);
00294                     break;
00295                 }
00296                 moppy.setFrequency(i, BIN_TO_FREQ(peak));
00297                 magnitudes[peak]*=0.5f;
00298                 if(buildSpectrumBuffer) {
00299                     spectrumBuffer[spectrumBufferLength++] = peak;
00300                 }
00301             }
00302             moppy.flush();
00303             
00304             if(buildSpectrumBuffer) {
00305                 spectrumBuffer[spectrumBufferLength++] = 0; //mark end of playing
00306                 //process time
00307                 long usedTime = timer.read_us();
00308                 long availTime = 1000000*FFT_SIZE/achievedSampleRate;
00309                 long percent = usedTime*10000/availTime; //fixed point to 0.01%
00310                 spectrumBuffer[spectrumBufferLength++] = percent/255;
00311                 spectrumBuffer[spectrumBufferLength++] = percent%255;
00312             }
00313             
00314             if(useFilter)
00315                 green = LED_OFF;
00316             else
00317                 blue = LED_OFF;
00318             
00319             sectionFilled = false;
00320         }
00321         
00322         if(spectrumBufferIndex<spectrumBufferLength) {
00323             pc.putc(spectrumBuffer[spectrumBufferIndex++]);
00324         } else {
00325             sleep();
00326         }
00327     }
00328 }