File content as of revision 1:02553973d9cf:

#include "mbed.h"
#include <algorithm>
#include "asyncADC.h"
#include "arm_math.h"
#include "arm_common_tables.h"
#include "moppy.h"

#define AUDIO_PIN       (A0)
 
Serial pc(USBTX, USBRX, 115200);
DigitalOut red(LED_RED), green(LED_GREEN), blue(LED_BLUE);
Timer timer;
InterruptIn filterBtn(SW2);
bool useFilter = false;

#define LED_ON      (0)
#define LED_OFF     (1)

#define FFT_SIZE 1024
#include "fft.h"

#define TARGET_SAMPLE_RATE_HZ  19200 // Sample rate in Hertz
float achievedSampleRate;

#define BIN_TO_FREQ(bin)  ((bin*(long)achievedSampleRate)/FFT_SIZE)

#define PC_BUFFER_SIZE (FFT_SIZE+30)

//BUFFERS

uint16_t adcBuffer[FFT_SIZE*2];
int spectrumBufferIndex = 0, spectrumBufferLength = 0;
char spectrumBuffer[PC_BUFFER_SIZE];
float fftBuffer[FFT_SIZE*2];
float magnitudes[FFT_SIZE];

// INTERRUPTS

volatile int sectionCounter = 0;
volatile bool sectionFilled = false, toggleFilter;
volatile uint32_t sectionStart, sectionEnd;

void callback(uint32_t start, uint32_t end) {
    sectionFilled = true;
    sectionCounter++;
    sectionStart = start;
    sectionEnd = end;
}

void press() {
    toggleFilter=true;
}

// UTILS

uint32_t ceilToPowerOf2(uint32_t i) {
    i--;
    i|=(i>>1);
    i|=(i>>2);
    i|=(i>>4);
    i|=(i>>8);
    i|=(i>>16);
    return i+1;
}

void trySwp(int16_t *indices, float *magnitudes, int a, int b) {
    if(magnitudes[indices[a]]>magnitudes[indices[b]]) {
        int x = indices[a];
        indices[a] = indices[b];
        indices[b] = x;
    }
}

void sort(int16_t *indices, float *magnitudes, int size) {
    indices[0] = 0;
    for(int i=1; i<size; i++) {
        indices[i]=i;
        for(int j=i; j-->0;) {
            trySwp(indices, magnitudes, j, j+1);
        }
    }
}

bool magComparator(int const & a, int const & b) {
    return magnitudes[a] > magnitudes[b];
}

// POST PROCESSING

float mqe(float* fft) {
    float t0_real, t0_imag, t1_real, t1_imag; //temp values
    
    t0_real = fft[0]-fft[4];
    t0_imag = fft[1]-fft[5];
    
    t1_real = fft[2]*2.0f - (fft[0] + fft[4]);
    t1_imag = fft[3]*2.0f - (fft[1] + fft[5]);
    
    return (t0_real*t1_real + t0_imag*t1_imag)/(t1_real*t1_real + t1_imag*t1_imag);
}

void modifiedQuadraticEstimator() {
    //Post process
    float prev = mqe(&fftBuffer[2]);
    for(int i=4; i<FFT_SIZE/2; i+=2) {
        //Eric Jacobsen's Modified Quadratic Estimator
        float curr = mqe(&fftBuffer[i]);
        fftBuffer[i-2]+=prev;
        prev = curr;
    }
    fftBuffer[FFT_SIZE/2]+=prev;
}

float getMag(int index) {
    return (index<1 && index>=FFT_SIZE/2) ? 0.0f : magnitudes[index];
}

#define FILTER_CUTOFF       (10000.0f)
void localMaximaRad10() {
    float p10, p9, p8, p7, p6, p5, p4, p3, p2, p1, c, n1, n2, n3, n4, n5, n6, n7, n8, n9, n10;
    c = getMag(1);
    n1 = getMag(2);
    n2 = getMag(3);
    n3 = getMag(4);
    n4 = getMag(5);
    n5 = getMag(6);
    n6 = getMag(7);
    n7 = getMag(8);
    n8 = getMag(9);
    n9 = getMag(10);
    n10 = getMag(11);
    
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    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);
    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;
    
    for(int i=11; i<FFT_SIZE/2; i++) {
        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);
        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;
    }
}
void localMaximaRad6() {
    float p6, p5, p4, p3, p2, p1, c, n1, n2, n3, n4, n5, n6;
    c = getMag(1);
    n1 = getMag(2);
    n2 = getMag(3);
    n3 = getMag(4);
    n4 = getMag(5);
    n5 = getMag(6);
    n6 = getMag(7);
    
    magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
    p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(5);
    magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
    p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(6);
    magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0;
    p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(7);
    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;
    p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(8);
    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;
    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);
    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;
    
    for(int i=7; i<FFT_SIZE/2; i++) {
        p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(i+6);
        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;
    }
}
void localMaximaRad3() {
    float p3, p2, p1, c, n1, n2, n3;
    c = getMag(1);
    n1 = getMag(2);
    n2 = getMag(3);
    n3 = getMag(4);
    
    magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
    p1=c; c=n1; n1=n2; n2=n3; n3=getMag(5);
    magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
    p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(6);
    magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
    
    for(int i=4; i<FFT_SIZE/2; i++) {
        p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(i+3);
        magnitudes[i] = (c>=FILTER_CUTOFF && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0;
    }
}

// MAIN

int main()
{
    filterBtn.fall(press);
    red   = LED_OFF;
    green = LED_OFF;
    blue  = LED_OFF;
    
    Moppy moppy(D1, D0, 38400); // tx, rx, baud
    
    achievedSampleRate = approximateFrequency((float)TARGET_SAMPLE_RATE_HZ);
    switch(asyncAnalogToCircularBuffer(AUDIO_PIN, adcBuffer, 2*FFT_SIZE, TARGET_SAMPLE_RATE_HZ, callback)) {
        case E_ASYNC_ADC_ACTIVE:
            error("AsyncADC already in use");
        case E_DMA_IN_USE:
            error("DMA already in use");
        case E_INVALID_BUFFER_SIZE:
            error("Invalid destination buffer size");
    }
    
    //fixed header
    spectrumBuffer[0]=255;
    spectrumBuffer[1]=(FFT_SIZE/2-1)/255;
    spectrumBuffer[2]=(FFT_SIZE/2-1)%255;
    
    timer.start();
    
    while(1) {
        if(toggleFilter) {
            useFilter = !useFilter;
            toggleFilter = false;
        }
        if(sectionFilled) {
            if(useFilter)
                green = LED_ON;
            else
                blue = LED_ON;
            
            bool buildSpectrumBuffer = false;
            
            if(spectrumBufferIndex==spectrumBufferLength) {
                buildSpectrumBuffer = true;
                spectrumBufferIndex = 0;
            }
            
            timer.reset();
            
            //convert to floating point
            for(int i=sectionStart, j=0; i<=sectionEnd; i++, j+=2) {
                fftBuffer[j] = (float)adcBuffer[i];
                fftBuffer[j+1] = 0.0f;
            }
                
            // Calculate FFT if a full sample is available.
            // Run FFT on sample data.
            arm_cfft_f32(FFT_BUFFER, fftBuffer, 0, 1);
            // Calculate magnitude of complex numbers output by the FFT.
            
            //Convert FFT complex vectors to magnitudes
            arm_cmplx_mag_f32(fftBuffer, magnitudes, FFT_SIZE);
            
            //Post Process / Filtering
            if(useFilter) {
                localMaximaRad10();
            } else {
                for(int i=FFT_SIZE/2; i-->0;) {
                    magnitudes[i]=magnitudes[i]<FILTER_CUTOFF ? 0.0f : sqrt(magnitudes[i]); //trivial amplitude filtering and normalise
                }
            }
            
            if(buildSpectrumBuffer) {
                spectrumBufferLength = 3;
                //data
                for(int i=1; i<(FFT_SIZE/2); i++) {
                    int m = ((int)magnitudes[i]+2)>>2;
                    spectrumBuffer[spectrumBufferLength++] = m>=254?254:m;
                }
            }
            
            for(int i=moppy.getFloppyDrives(); i-->0;) {
                int peak = 1;
                for(int j=FFT_SIZE/2; j-->2;) {
                    if(magnitudes[j]>magnitudes[peak])
                        peak=j;
                }
                if(magnitudes[peak]<30.0f) {
                    moppy.setFrequency(i, 0);
                    while(i-->0)
                        moppy.setFrequency(i, 0);
                    break;
                }
                moppy.setFrequency(i, BIN_TO_FREQ(peak));
                magnitudes[peak]*=0.5f;
                if(buildSpectrumBuffer) {
                    spectrumBuffer[spectrumBufferLength++] = peak;
                }
            }
            moppy.flush();
            
            if(buildSpectrumBuffer) {
                spectrumBuffer[spectrumBufferLength++] = 0; //mark end of playing
                //process time
                long usedTime = timer.read_us();
                long availTime = 1000000*FFT_SIZE/achievedSampleRate;
                long percent = usedTime*10000/availTime; //fixed point to 0.01%
                spectrumBuffer[spectrumBufferLength++] = percent/255;
                spectrumBuffer[spectrumBufferLength++] = percent%255;
            }
            
            if(useFilter)
                green = LED_OFF;
            else
                blue = LED_OFF;
            
            sectionFilled = false;
        }
        
        if(spectrumBufferIndex<spectrumBufferLength) {
            pc.putc(spectrumBuffer[spectrumBufferIndex++]);
        } else {
            sleep();
        }
    }
}