An floppy drive audio generator using dsp on live audio
Dependencies: Terminal asyncADC mbed-dsp mbed
main2.cpp@0:84c336a81482, 2017-05-24 (annotated)
- Committer:
- Condo2k4
- Date:
- Wed May 24 11:58:43 2017 +0000
- Revision:
- 0:84c336a81482
Initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Condo2k4 | 0:84c336a81482 | 1 | #include "mbed.h" |
Condo2k4 | 0:84c336a81482 | 2 | #include <algorithm> |
Condo2k4 | 0:84c336a81482 | 3 | #include "asyncADC.h" |
Condo2k4 | 0:84c336a81482 | 4 | #include "arm_math.h" |
Condo2k4 | 0:84c336a81482 | 5 | #include "arm_common_tables.h" |
Condo2k4 | 0:84c336a81482 | 6 | #include "moppy.h" |
Condo2k4 | 0:84c336a81482 | 7 | |
Condo2k4 | 0:84c336a81482 | 8 | #define AUDIO_PIN (A0) |
Condo2k4 | 0:84c336a81482 | 9 | |
Condo2k4 | 0:84c336a81482 | 10 | Serial pc(USBTX, USBRX, 115200); |
Condo2k4 | 0:84c336a81482 | 11 | DigitalOut red(LED_RED), green(LED_GREEN), blue(LED_BLUE); |
Condo2k4 | 0:84c336a81482 | 12 | Timer timer; |
Condo2k4 | 0:84c336a81482 | 13 | InterruptIn filterBtn(SW2); |
Condo2k4 | 0:84c336a81482 | 14 | bool useFilter = false; |
Condo2k4 | 0:84c336a81482 | 15 | |
Condo2k4 | 0:84c336a81482 | 16 | #define LED_ON (0) |
Condo2k4 | 0:84c336a81482 | 17 | #define LED_OFF (1) |
Condo2k4 | 0:84c336a81482 | 18 | |
Condo2k4 | 0:84c336a81482 | 19 | #define FFT_SIZE 1024 |
Condo2k4 | 0:84c336a81482 | 20 | #include "fft.h" |
Condo2k4 | 0:84c336a81482 | 21 | |
Condo2k4 | 0:84c336a81482 | 22 | #define TARGET_SAMPLE_RATE_HZ 19200 // Sample rate in Hertz |
Condo2k4 | 0:84c336a81482 | 23 | float achievedSampleRate; |
Condo2k4 | 0:84c336a81482 | 24 | |
Condo2k4 | 0:84c336a81482 | 25 | #define BIN_TO_FREQ(bin) ((bin*(long)achievedSampleRate)/FFT_SIZE) |
Condo2k4 | 0:84c336a81482 | 26 | |
Condo2k4 | 0:84c336a81482 | 27 | #define PC_BUFFER_SIZE (FFT_SIZE+30) |
Condo2k4 | 0:84c336a81482 | 28 | |
Condo2k4 | 0:84c336a81482 | 29 | //BUFFERS |
Condo2k4 | 0:84c336a81482 | 30 | |
Condo2k4 | 0:84c336a81482 | 31 | uint16_t adcBuffer[FFT_SIZE*2]; |
Condo2k4 | 0:84c336a81482 | 32 | int spectrumBufferIndex = 0, spectrumBufferLength = 0; |
Condo2k4 | 0:84c336a81482 | 33 | char spectrumBuffer[PC_BUFFER_SIZE]; |
Condo2k4 | 0:84c336a81482 | 34 | float fftBuffer[FFT_SIZE*2]; |
Condo2k4 | 0:84c336a81482 | 35 | float magnitudes[FFT_SIZE]; |
Condo2k4 | 0:84c336a81482 | 36 | |
Condo2k4 | 0:84c336a81482 | 37 | // INTERRUPTS |
Condo2k4 | 0:84c336a81482 | 38 | |
Condo2k4 | 0:84c336a81482 | 39 | volatile int sectionCounter = 0; |
Condo2k4 | 0:84c336a81482 | 40 | volatile bool sectionFilled = false, toggleFilter; |
Condo2k4 | 0:84c336a81482 | 41 | volatile uint32_t sectionStart, sectionEnd; |
Condo2k4 | 0:84c336a81482 | 42 | |
Condo2k4 | 0:84c336a81482 | 43 | void callback(uint32_t start, uint32_t end) { |
Condo2k4 | 0:84c336a81482 | 44 | sectionFilled = true; |
Condo2k4 | 0:84c336a81482 | 45 | sectionCounter++; |
Condo2k4 | 0:84c336a81482 | 46 | sectionStart = start; |
Condo2k4 | 0:84c336a81482 | 47 | sectionEnd = end; |
Condo2k4 | 0:84c336a81482 | 48 | } |
Condo2k4 | 0:84c336a81482 | 49 | |
Condo2k4 | 0:84c336a81482 | 50 | void press() { |
Condo2k4 | 0:84c336a81482 | 51 | toggleFilter=true; |
Condo2k4 | 0:84c336a81482 | 52 | } |
Condo2k4 | 0:84c336a81482 | 53 | |
Condo2k4 | 0:84c336a81482 | 54 | // UTILS |
Condo2k4 | 0:84c336a81482 | 55 | |
Condo2k4 | 0:84c336a81482 | 56 | uint32_t ceilToPowerOf2(uint32_t i) { |
Condo2k4 | 0:84c336a81482 | 57 | i--; |
Condo2k4 | 0:84c336a81482 | 58 | i|=(i>>1); |
Condo2k4 | 0:84c336a81482 | 59 | i|=(i>>2); |
Condo2k4 | 0:84c336a81482 | 60 | i|=(i>>4); |
Condo2k4 | 0:84c336a81482 | 61 | i|=(i>>8); |
Condo2k4 | 0:84c336a81482 | 62 | i|=(i>>16); |
Condo2k4 | 0:84c336a81482 | 63 | return i+1; |
Condo2k4 | 0:84c336a81482 | 64 | } |
Condo2k4 | 0:84c336a81482 | 65 | |
Condo2k4 | 0:84c336a81482 | 66 | void trySwp(int16_t *indices, float *magnitudes, int a, int b) { |
Condo2k4 | 0:84c336a81482 | 67 | if(magnitudes[indices[a]]>magnitudes[indices[b]]) { |
Condo2k4 | 0:84c336a81482 | 68 | int x = indices[a]; |
Condo2k4 | 0:84c336a81482 | 69 | indices[a] = indices[b]; |
Condo2k4 | 0:84c336a81482 | 70 | indices[b] = x; |
Condo2k4 | 0:84c336a81482 | 71 | } |
Condo2k4 | 0:84c336a81482 | 72 | } |
Condo2k4 | 0:84c336a81482 | 73 | |
Condo2k4 | 0:84c336a81482 | 74 | void sort(int16_t *indices, float *magnitudes, int size) { |
Condo2k4 | 0:84c336a81482 | 75 | indices[0] = 0; |
Condo2k4 | 0:84c336a81482 | 76 | for(int i=1; i<size; i++) { |
Condo2k4 | 0:84c336a81482 | 77 | indices[i]=i; |
Condo2k4 | 0:84c336a81482 | 78 | for(int j=i; j-->0;) { |
Condo2k4 | 0:84c336a81482 | 79 | trySwp(indices, magnitudes, j, j+1); |
Condo2k4 | 0:84c336a81482 | 80 | } |
Condo2k4 | 0:84c336a81482 | 81 | } |
Condo2k4 | 0:84c336a81482 | 82 | } |
Condo2k4 | 0:84c336a81482 | 83 | |
Condo2k4 | 0:84c336a81482 | 84 | bool magComparator(int const & a, int const & b) { |
Condo2k4 | 0:84c336a81482 | 85 | return magnitudes[a] > magnitudes[b]; |
Condo2k4 | 0:84c336a81482 | 86 | } |
Condo2k4 | 0:84c336a81482 | 87 | |
Condo2k4 | 0:84c336a81482 | 88 | // POST PROCESSING |
Condo2k4 | 0:84c336a81482 | 89 | |
Condo2k4 | 0:84c336a81482 | 90 | float mqe(float* fft) { |
Condo2k4 | 0:84c336a81482 | 91 | float t0_real, t0_imag, t1_real, t1_imag; //temp values |
Condo2k4 | 0:84c336a81482 | 92 | |
Condo2k4 | 0:84c336a81482 | 93 | t0_real = fft[0]-fft[4]; |
Condo2k4 | 0:84c336a81482 | 94 | t0_imag = fft[1]-fft[5]; |
Condo2k4 | 0:84c336a81482 | 95 | |
Condo2k4 | 0:84c336a81482 | 96 | t1_real = fft[2]*2.0f - (fft[0] + fft[4]); |
Condo2k4 | 0:84c336a81482 | 97 | t1_imag = fft[3]*2.0f - (fft[1] + fft[5]); |
Condo2k4 | 0:84c336a81482 | 98 | |
Condo2k4 | 0:84c336a81482 | 99 | return (t0_real*t1_real + t0_imag*t1_imag)/(t1_real*t1_real + t1_imag*t1_imag); |
Condo2k4 | 0:84c336a81482 | 100 | } |
Condo2k4 | 0:84c336a81482 | 101 | |
Condo2k4 | 0:84c336a81482 | 102 | void modifiedQuadraticEstimator() { |
Condo2k4 | 0:84c336a81482 | 103 | //Post process |
Condo2k4 | 0:84c336a81482 | 104 | float prev = mqe(&fftBuffer[2]); |
Condo2k4 | 0:84c336a81482 | 105 | for(int i=4; i<FFT_SIZE/2; i+=2) { |
Condo2k4 | 0:84c336a81482 | 106 | //Eric Jacobsen's Modified Quadratic Estimator |
Condo2k4 | 0:84c336a81482 | 107 | float curr = mqe(&fftBuffer[i]); |
Condo2k4 | 0:84c336a81482 | 108 | fftBuffer[i-2]+=prev; |
Condo2k4 | 0:84c336a81482 | 109 | prev = curr; |
Condo2k4 | 0:84c336a81482 | 110 | } |
Condo2k4 | 0:84c336a81482 | 111 | fftBuffer[FFT_SIZE/2]+=prev; |
Condo2k4 | 0:84c336a81482 | 112 | } |
Condo2k4 | 0:84c336a81482 | 113 | |
Condo2k4 | 0:84c336a81482 | 114 | float getMag(int index) { |
Condo2k4 | 0:84c336a81482 | 115 | return (index<1 && index>=FFT_SIZE/2) ? 0.0f : magnitudes[index]; |
Condo2k4 | 0:84c336a81482 | 116 | } |
Condo2k4 | 0:84c336a81482 | 117 | |
Condo2k4 | 0:84c336a81482 | 118 | #define FILTER_CUTOFF (10000.0f) |
Condo2k4 | 0:84c336a81482 | 119 | void localMaximaRad10() { |
Condo2k4 | 0:84c336a81482 | 120 | float p10, p9, p8, p7, p6, p5, p4, p3, p2, p1, c, n1, n2, n3, n4, n5, n6, n7, n8, n9, n10; |
Condo2k4 | 0:84c336a81482 | 121 | c = getMag(1); |
Condo2k4 | 0:84c336a81482 | 122 | n1 = getMag(2); |
Condo2k4 | 0:84c336a81482 | 123 | n2 = getMag(3); |
Condo2k4 | 0:84c336a81482 | 124 | n3 = getMag(4); |
Condo2k4 | 0:84c336a81482 | 125 | n4 = getMag(5); |
Condo2k4 | 0:84c336a81482 | 126 | n5 = getMag(6); |
Condo2k4 | 0:84c336a81482 | 127 | n6 = getMag(7); |
Condo2k4 | 0:84c336a81482 | 128 | n7 = getMag(8); |
Condo2k4 | 0:84c336a81482 | 129 | n8 = getMag(9); |
Condo2k4 | 0:84c336a81482 | 130 | n9 = getMag(10); |
Condo2k4 | 0:84c336a81482 | 131 | n10 = getMag(11); |
Condo2k4 | 0:84c336a81482 | 132 | |
Condo2k4 | 0:84c336a81482 | 133 | 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; |
Condo2k4 | 0:84c336a81482 | 134 | 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); |
Condo2k4 | 0:84c336a81482 | 135 | 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; |
Condo2k4 | 0:84c336a81482 | 136 | 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); |
Condo2k4 | 0:84c336a81482 | 137 | 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; |
Condo2k4 | 0:84c336a81482 | 138 | 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); |
Condo2k4 | 0:84c336a81482 | 139 | 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; |
Condo2k4 | 0:84c336a81482 | 140 | 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); |
Condo2k4 | 0:84c336a81482 | 141 | 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; |
Condo2k4 | 0:84c336a81482 | 142 | 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); |
Condo2k4 | 0:84c336a81482 | 143 | 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; |
Condo2k4 | 0:84c336a81482 | 144 | 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); |
Condo2k4 | 0:84c336a81482 | 145 | 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; |
Condo2k4 | 0:84c336a81482 | 146 | 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); |
Condo2k4 | 0:84c336a81482 | 147 | 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; |
Condo2k4 | 0:84c336a81482 | 148 | 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); |
Condo2k4 | 0:84c336a81482 | 149 | 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; |
Condo2k4 | 0:84c336a81482 | 150 | 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); |
Condo2k4 | 0:84c336a81482 | 151 | 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; |
Condo2k4 | 0:84c336a81482 | 152 | |
Condo2k4 | 0:84c336a81482 | 153 | for(int i=11; i<FFT_SIZE/2; i++) { |
Condo2k4 | 0:84c336a81482 | 154 | 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); |
Condo2k4 | 0:84c336a81482 | 155 | 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; |
Condo2k4 | 0:84c336a81482 | 156 | } |
Condo2k4 | 0:84c336a81482 | 157 | } |
Condo2k4 | 0:84c336a81482 | 158 | void localMaximaRad6() { |
Condo2k4 | 0:84c336a81482 | 159 | float p6, p5, p4, p3, p2, p1, c, n1, n2, n3, n4, n5, n6; |
Condo2k4 | 0:84c336a81482 | 160 | c = getMag(1); |
Condo2k4 | 0:84c336a81482 | 161 | n1 = getMag(2); |
Condo2k4 | 0:84c336a81482 | 162 | n2 = getMag(3); |
Condo2k4 | 0:84c336a81482 | 163 | n3 = getMag(4); |
Condo2k4 | 0:84c336a81482 | 164 | n4 = getMag(5); |
Condo2k4 | 0:84c336a81482 | 165 | n5 = getMag(6); |
Condo2k4 | 0:84c336a81482 | 166 | n6 = getMag(7); |
Condo2k4 | 0:84c336a81482 | 167 | |
Condo2k4 | 0:84c336a81482 | 168 | magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 169 | p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(5); |
Condo2k4 | 0:84c336a81482 | 170 | magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 171 | p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(6); |
Condo2k4 | 0:84c336a81482 | 172 | magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3 && c>=n4 && c>=n5 && c>=n6) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 173 | p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(7); |
Condo2k4 | 0:84c336a81482 | 174 | 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; |
Condo2k4 | 0:84c336a81482 | 175 | p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=n4; n4=n5; n5=n6; n6=getMag(8); |
Condo2k4 | 0:84c336a81482 | 176 | 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; |
Condo2k4 | 0:84c336a81482 | 177 | 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); |
Condo2k4 | 0:84c336a81482 | 178 | 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; |
Condo2k4 | 0:84c336a81482 | 179 | |
Condo2k4 | 0:84c336a81482 | 180 | for(int i=7; i<FFT_SIZE/2; i++) { |
Condo2k4 | 0:84c336a81482 | 181 | p6=p5; p5=p4; p4=p3; p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(i+6); |
Condo2k4 | 0:84c336a81482 | 182 | 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; |
Condo2k4 | 0:84c336a81482 | 183 | } |
Condo2k4 | 0:84c336a81482 | 184 | } |
Condo2k4 | 0:84c336a81482 | 185 | void localMaximaRad3() { |
Condo2k4 | 0:84c336a81482 | 186 | float p3, p2, p1, c, n1, n2, n3; |
Condo2k4 | 0:84c336a81482 | 187 | c = getMag(1); |
Condo2k4 | 0:84c336a81482 | 188 | n1 = getMag(2); |
Condo2k4 | 0:84c336a81482 | 189 | n2 = getMag(3); |
Condo2k4 | 0:84c336a81482 | 190 | n3 = getMag(4); |
Condo2k4 | 0:84c336a81482 | 191 | |
Condo2k4 | 0:84c336a81482 | 192 | magnitudes[1] = (c>=FILTER_CUTOFF && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 193 | p1=c; c=n1; n1=n2; n2=n3; n3=getMag(5); |
Condo2k4 | 0:84c336a81482 | 194 | magnitudes[2] = (c>=FILTER_CUTOFF && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 195 | p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(6); |
Condo2k4 | 0:84c336a81482 | 196 | magnitudes[3] = (c>=FILTER_CUTOFF && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 197 | |
Condo2k4 | 0:84c336a81482 | 198 | for(int i=4; i<FFT_SIZE/2; i++) { |
Condo2k4 | 0:84c336a81482 | 199 | p3=p2; p2=p1; p1=c; c=n1; n1=n2; n2=n3; n3=getMag(i+3); |
Condo2k4 | 0:84c336a81482 | 200 | magnitudes[i] = (c>=FILTER_CUTOFF && c>p3 && c>p2 && c>p1 && c>=n1 && c>=n2 && c>=n3) ? sqrt(c) : 0; |
Condo2k4 | 0:84c336a81482 | 201 | } |
Condo2k4 | 0:84c336a81482 | 202 | } |
Condo2k4 | 0:84c336a81482 | 203 | |
Condo2k4 | 0:84c336a81482 | 204 | // MAIN |
Condo2k4 | 0:84c336a81482 | 205 | |
Condo2k4 | 0:84c336a81482 | 206 | int main() |
Condo2k4 | 0:84c336a81482 | 207 | { |
Condo2k4 | 0:84c336a81482 | 208 | filterBtn.fall(press); |
Condo2k4 | 0:84c336a81482 | 209 | red = LED_OFF; |
Condo2k4 | 0:84c336a81482 | 210 | green = LED_OFF; |
Condo2k4 | 0:84c336a81482 | 211 | blue = LED_OFF; |
Condo2k4 | 0:84c336a81482 | 212 | |
Condo2k4 | 0:84c336a81482 | 213 | Moppy moppy(D1, D0, 38400); // tx, rx, baud |
Condo2k4 | 0:84c336a81482 | 214 | |
Condo2k4 | 0:84c336a81482 | 215 | achievedSampleRate = approximateFrequency((float)TARGET_SAMPLE_RATE_HZ); |
Condo2k4 | 0:84c336a81482 | 216 | switch(asyncAnalogToCircularBuffer(AUDIO_PIN, adcBuffer, 2*FFT_SIZE, TARGET_SAMPLE_RATE_HZ, callback)) { |
Condo2k4 | 0:84c336a81482 | 217 | case E_ASYNC_ADC_ACTIVE: |
Condo2k4 | 0:84c336a81482 | 218 | error("AsyncADC already in use"); |
Condo2k4 | 0:84c336a81482 | 219 | case E_DMA_IN_USE: |
Condo2k4 | 0:84c336a81482 | 220 | error("DMA already in use"); |
Condo2k4 | 0:84c336a81482 | 221 | case E_INVALID_BUFFER_SIZE: |
Condo2k4 | 0:84c336a81482 | 222 | error("Invalid destination buffer size"); |
Condo2k4 | 0:84c336a81482 | 223 | } |
Condo2k4 | 0:84c336a81482 | 224 | |
Condo2k4 | 0:84c336a81482 | 225 | //fixed header |
Condo2k4 | 0:84c336a81482 | 226 | spectrumBuffer[0]=255; |
Condo2k4 | 0:84c336a81482 | 227 | spectrumBuffer[1]=(FFT_SIZE/2-1)/255; |
Condo2k4 | 0:84c336a81482 | 228 | spectrumBuffer[2]=(FFT_SIZE/2-1)%255; |
Condo2k4 | 0:84c336a81482 | 229 | |
Condo2k4 | 0:84c336a81482 | 230 | timer.start(); |
Condo2k4 | 0:84c336a81482 | 231 | |
Condo2k4 | 0:84c336a81482 | 232 | while(1) { |
Condo2k4 | 0:84c336a81482 | 233 | if(toggleFilter) { |
Condo2k4 | 0:84c336a81482 | 234 | useFilter = !useFilter; |
Condo2k4 | 0:84c336a81482 | 235 | toggleFilter = false; |
Condo2k4 | 0:84c336a81482 | 236 | } |
Condo2k4 | 0:84c336a81482 | 237 | if(sectionFilled) { |
Condo2k4 | 0:84c336a81482 | 238 | if(useFilter) |
Condo2k4 | 0:84c336a81482 | 239 | green = LED_ON; |
Condo2k4 | 0:84c336a81482 | 240 | else |
Condo2k4 | 0:84c336a81482 | 241 | blue = LED_ON; |
Condo2k4 | 0:84c336a81482 | 242 | |
Condo2k4 | 0:84c336a81482 | 243 | bool buildSpectrumBuffer = false; |
Condo2k4 | 0:84c336a81482 | 244 | |
Condo2k4 | 0:84c336a81482 | 245 | if(spectrumBufferIndex==spectrumBufferLength) { |
Condo2k4 | 0:84c336a81482 | 246 | buildSpectrumBuffer = true; |
Condo2k4 | 0:84c336a81482 | 247 | spectrumBufferIndex = 0; |
Condo2k4 | 0:84c336a81482 | 248 | } |
Condo2k4 | 0:84c336a81482 | 249 | |
Condo2k4 | 0:84c336a81482 | 250 | timer.reset(); |
Condo2k4 | 0:84c336a81482 | 251 | |
Condo2k4 | 0:84c336a81482 | 252 | //convert to floating point |
Condo2k4 | 0:84c336a81482 | 253 | for(int i=sectionStart, j=0; i<=sectionEnd; i++, j+=2) { |
Condo2k4 | 0:84c336a81482 | 254 | fftBuffer[j] = (float)adcBuffer[i]; |
Condo2k4 | 0:84c336a81482 | 255 | fftBuffer[j+1] = 0.0f; |
Condo2k4 | 0:84c336a81482 | 256 | } |
Condo2k4 | 0:84c336a81482 | 257 | |
Condo2k4 | 0:84c336a81482 | 258 | // Calculate FFT if a full sample is available. |
Condo2k4 | 0:84c336a81482 | 259 | // Run FFT on sample data. |
Condo2k4 | 0:84c336a81482 | 260 | arm_cfft_f32(FFT_BUFFER, fftBuffer, 0, 1); |
Condo2k4 | 0:84c336a81482 | 261 | // Calculate magnitude of complex numbers output by the FFT. |
Condo2k4 | 0:84c336a81482 | 262 | |
Condo2k4 | 0:84c336a81482 | 263 | //Convert FFT complex vectors to magnitudes |
Condo2k4 | 0:84c336a81482 | 264 | arm_cmplx_mag_f32(fftBuffer, magnitudes, FFT_SIZE); |
Condo2k4 | 0:84c336a81482 | 265 | |
Condo2k4 | 0:84c336a81482 | 266 | //Post Process / Filtering |
Condo2k4 | 0:84c336a81482 | 267 | if(useFilter) { |
Condo2k4 | 0:84c336a81482 | 268 | localMaximaRad10(); |
Condo2k4 | 0:84c336a81482 | 269 | } else { |
Condo2k4 | 0:84c336a81482 | 270 | for(int i=FFT_SIZE/2; i-->0;) { |
Condo2k4 | 0:84c336a81482 | 271 | magnitudes[i]=magnitudes[i]<FILTER_CUTOFF ? 0.0f : sqrt(magnitudes[i]); //trivial amplitude filtering and normalise |
Condo2k4 | 0:84c336a81482 | 272 | } |
Condo2k4 | 0:84c336a81482 | 273 | } |
Condo2k4 | 0:84c336a81482 | 274 | |
Condo2k4 | 0:84c336a81482 | 275 | if(buildSpectrumBuffer) { |
Condo2k4 | 0:84c336a81482 | 276 | spectrumBufferLength = 3; |
Condo2k4 | 0:84c336a81482 | 277 | //data |
Condo2k4 | 0:84c336a81482 | 278 | for(int i=1; i<(FFT_SIZE/2); i++) { |
Condo2k4 | 0:84c336a81482 | 279 | int m = ((int)magnitudes[i]+2)>>2; |
Condo2k4 | 0:84c336a81482 | 280 | spectrumBuffer[spectrumBufferLength++] = m>=254?254:m; |
Condo2k4 | 0:84c336a81482 | 281 | } |
Condo2k4 | 0:84c336a81482 | 282 | } |
Condo2k4 | 0:84c336a81482 | 283 | |
Condo2k4 | 0:84c336a81482 | 284 | for(int i=moppy.getFloppyDrives(); i-->0;) { |
Condo2k4 | 0:84c336a81482 | 285 | int peak = 1; |
Condo2k4 | 0:84c336a81482 | 286 | for(int j=FFT_SIZE/2; j-->2;) { |
Condo2k4 | 0:84c336a81482 | 287 | if(magnitudes[j]>magnitudes[peak]) |
Condo2k4 | 0:84c336a81482 | 288 | peak=j; |
Condo2k4 | 0:84c336a81482 | 289 | } |
Condo2k4 | 0:84c336a81482 | 290 | if(magnitudes[peak]<30.0f) { |
Condo2k4 | 0:84c336a81482 | 291 | moppy.setFrequency(i, 0); |
Condo2k4 | 0:84c336a81482 | 292 | while(i-->0) |
Condo2k4 | 0:84c336a81482 | 293 | moppy.setFrequency(i, 0); |
Condo2k4 | 0:84c336a81482 | 294 | break; |
Condo2k4 | 0:84c336a81482 | 295 | } |
Condo2k4 | 0:84c336a81482 | 296 | moppy.setFrequency(i, BIN_TO_FREQ(peak)); |
Condo2k4 | 0:84c336a81482 | 297 | magnitudes[peak]*=0.5f; |
Condo2k4 | 0:84c336a81482 | 298 | if(buildSpectrumBuffer) { |
Condo2k4 | 0:84c336a81482 | 299 | spectrumBuffer[spectrumBufferLength++] = peak; |
Condo2k4 | 0:84c336a81482 | 300 | } |
Condo2k4 | 0:84c336a81482 | 301 | } |
Condo2k4 | 0:84c336a81482 | 302 | moppy.flush(); |
Condo2k4 | 0:84c336a81482 | 303 | |
Condo2k4 | 0:84c336a81482 | 304 | if(buildSpectrumBuffer) { |
Condo2k4 | 0:84c336a81482 | 305 | spectrumBuffer[spectrumBufferLength++] = 0; //mark end of playing |
Condo2k4 | 0:84c336a81482 | 306 | //process time |
Condo2k4 | 0:84c336a81482 | 307 | long usedTime = timer.read_us(); |
Condo2k4 | 0:84c336a81482 | 308 | long availTime = 1000000*FFT_SIZE/achievedSampleRate; |
Condo2k4 | 0:84c336a81482 | 309 | long percent = usedTime*10000/availTime; //fixed point to 0.01% |
Condo2k4 | 0:84c336a81482 | 310 | spectrumBuffer[spectrumBufferLength++] = percent/255; |
Condo2k4 | 0:84c336a81482 | 311 | spectrumBuffer[spectrumBufferLength++] = percent%255; |
Condo2k4 | 0:84c336a81482 | 312 | } |
Condo2k4 | 0:84c336a81482 | 313 | |
Condo2k4 | 0:84c336a81482 | 314 | if(useFilter) |
Condo2k4 | 0:84c336a81482 | 315 | green = LED_OFF; |
Condo2k4 | 0:84c336a81482 | 316 | else |
Condo2k4 | 0:84c336a81482 | 317 | blue = LED_OFF; |
Condo2k4 | 0:84c336a81482 | 318 | |
Condo2k4 | 0:84c336a81482 | 319 | sectionFilled = false; |
Condo2k4 | 0:84c336a81482 | 320 | } |
Condo2k4 | 0:84c336a81482 | 321 | |
Condo2k4 | 0:84c336a81482 | 322 | if(spectrumBufferIndex<spectrumBufferLength) { |
Condo2k4 | 0:84c336a81482 | 323 | pc.putc(spectrumBuffer[spectrumBufferIndex++]); |
Condo2k4 | 0:84c336a81482 | 324 | } else { |
Condo2k4 | 0:84c336a81482 | 325 | sleep(); |
Condo2k4 | 0:84c336a81482 | 326 | } |
Condo2k4 | 0:84c336a81482 | 327 | } |
Condo2k4 | 0:84c336a81482 | 328 | } |