Keith G
An floppy drive audio generator using dsp on live audio
Dependencies: Terminal asyncADC mbed-dsp mbed
Diff: main2.cpp
- Revision:
- 0:84c336a81482
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main2.cpp Wed May 24 11:58:43 2017 +0000
@@ -0,0 +1,328 @@
+#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();
+ }
+ }
+}
\ No newline at end of file