detect speed from fft spectrum, draw in graph

FFT_visualisation/FFT_visualisation.pde

@@ -6,17 +6,18 @@ int xStart = 70;
 int xPos = xStart;         // horizontal position of the graph
 float speed = 0;
 float old_speed = 0;
-int num_fft_bins = 200;
+int num_fft_bins = 511;
 float[] nums = new float[num_fft_bins+2];
 float spec_speed = 0;
-int sample_rate = 11000;
+int sample_rate = 44100;
 float fft_bin_width = sample_rate/1024;
 float speed_conversion = fft_bin_width/44.0; //44100/1024/44; //1.578125;
 int max_frequency = 100;
 
 String inString;
 float peak;
 float oldpeak = 0;
+int baseline;
 
 
 void setup () {
@@ -44,28 +45,31 @@ void setup () {
 
   draw_axis();
 
+
+
 }
 
 void draw_axis () { 
+  baseline = height-50;
   // axis
   stroke(#FFFFFF);
   fill(#FFFFFF);
   //rect(0, 0, xStart-1, height);
   background(255);
 
   stroke(#ff0000);
-  line(xStart-1, 0, xStart-1, height);
-  for(int s =0; s < 1000; s=s+10){
+  line(xStart-1, 0, xStart-1, baseline);
+  for(int s =0; s < 1000; s=s+5){
     int tick_label = s;
-    float tick_position = map(tick_label/speed_conversion, 1, max_frequency, height, 0);
+    float tick_position = map(tick_label/speed_conversion, 0, max_frequency, baseline, 0);
     line(xStart-10, tick_position, xStart-1, tick_position);
     fill(#ff0000);
     textSize(20);
     textAlign(RIGHT,CENTER);
     text(tick_label, xStart-13, tick_position-5);
   }
   textAlign(CENTER, CENTER);
-  translate(10, height/2);
+  translate(10, baseline/2);
   rotate(radians(-90));
   text("Geschwindigkeit (km/h)", 0, 0);
 
@@ -78,17 +82,17 @@ void draw () {
   // draw waterfall spectrogramm
   for(int i = 0; i < max_frequency; i++){
     //fill(xPos, 100, nums[i]);
-    spec_speed = i+2.5;
-    spec_speed = map(spec_speed, 1, max_frequency, 0, height);
+    spec_speed = i;
+    spec_speed = map(spec_speed, 0, max_frequency, 0, baseline);
     stroke(255-nums[i]*10);
-    rect(xPos, height-spec_speed, 1, height/max_frequency);
+    rect(xPos, baseline-spec_speed, 1, -baseline/max_frequency);
   }
 
   // draw the line:
   stroke(#ff0000);
-  if(peak>0.01){
-    //line(xPos-1, height-old_speed, xPos, height - speed);
-  }
+  //if(peak>0.05){
+    line(xPos-1, map(old_speed/speed_conversion, 0, max_frequency, baseline, 0), xPos, map(speed/speed_conversion, 0, max_frequency, baseline, 0));
+  //}
 
   // at the edge of the screen, go back to the beginning:
   if (xPos >= width) {
@@ -156,14 +160,8 @@ void serialEvent (Serial myPort) {
 
   if (inString != null) {
     old_speed = speed;
-
     nums = float(trim(split(inString, ',')));
-    // trim off any whitespace:
-    //println(nums[155]);
-    if(nums[num_fft_bins+1]>0.0 & nums[num_fft_bins+1]/44 < 150){
-      speed = map(nums[num_fft_bins+1], 1, max_frequency, 0, height) + fft_bin_width/44;
-      //speed = map(4, 0, max_frequency, 0, height);
-    }
+    speed = nums[num_fft_bins+1];
     peak = nums[num_fft_bins+2];
     oldpeak = max(peak, oldpeak);
     println(round(oldpeak*100)/100.0);

Teensy_fft.ino

@@ -1,17 +1,10 @@
-/*
- * A simple hardware test which receives audio from the audio shield
- * Line-In pins and send it to the Line-Out pins and headphone jack.
- *
- * This example code is in the public domain.
- */
-
 #include <Audio.h>
 #include <Wire.h>
 #include <SPI.h>
 #include <SD.h>
 #include <SerialFlash.h>
 #include <utility/imxrt_hw.h>
-  
+
 void setI2SFreq(int freq) {
   // PLL between 27*24 = 648MHz und 54*24=1296MHz
   int n1 = 4; //SAI prescaler 4 => (n1*n2) = multiple of 4
@@ -24,73 +17,31 @@ void setI2SFreq(int freq) {
   CCM_CS1CDR = (CCM_CS1CDR & ~(CCM_CS1CDR_SAI1_CLK_PRED_MASK | CCM_CS1CDR_SAI1_CLK_PODF_MASK))
        | CCM_CS1CDR_SAI1_CLK_PRED(n1-1) // &0x07
        | CCM_CS1CDR_SAI1_CLK_PODF(n2-1); // &0x3f 
-//Serial.printf("SetI2SFreq(%d)\n",freq);
 }
 
-AudioAnalyzeFFT1024      fft1024_1;      //xy=862.3333129882812,439.33331298828125
-AudioAmplifier           amp1;           //xy=425,443
-AudioAnalyzePeak         peak1;          //xy=507,307
-AudioFilterFIR           fir1; 
-AudioAnalyzeNoteFrequency notefreq1;      
-AudioMixer4               mixer;
+const int sample_rate = 44100;
+uint16_t max_amplitude;                   //highest signal in spectrum              
+uint16_t max_freq_Index;
+float speed_conversion = (sample_rate/1024)/44.0;
 
+AudioAnalyzeFFT1024      fft1024;      
+AudioAmplifier           amp1;           
+AudioAnalyzePeak         peak1;          
+AudioMixer4              mixer;
 
-AudioInputI2S            mic;           //xy=200,69
-AudioOutputI2S           headphone;           //xy=365,94
+AudioInputI2S            mic;           
+AudioOutputI2S           headphone;           
 AudioConnection          patchCord1(mic, 0, mixer, 0);
-//AudioConnection          patchCord2(mic, 1, mixer, 1);
-AudioControlSGTL5000     sgtl5000_1;     //xy=302,184
+AudioControlSGTL5000     sgtl5000_1;     
 AudioConnection          patchCord3(mic, amp1);
-AudioConnection          patchCord4(amp1, 0, fft1024_1, 0);
+AudioConnection          patchCord4(amp1, 0, fft1024, 0);
 AudioConnection          patchCord5(mixer, 0, peak1, 0);
 AudioConnection          patchCord6(mixer, 0, headphone, 0);
 AudioConnection          patchCord7(mixer, 0, headphone, 1);
-AudioConnection          patchCord8(mixer, 0, notefreq1, 0);
-// GUItool: end automatically generated code
-
-#define TAP_NUM   36
-short filt_coeff[TAP_NUM] = {
-  28,
-  -2,
-  -108,
-  -287,
-  -434,
-  -387,
-  -53,
-  449,
-  783,
-  578,
-  -250,
-  -1290,
-  -1727,
-  -791,
-  1697,
-  5046,
-  7932,
-  9070,
-  7932,
-  5046,
-  1697,
-  -791,
-  -1727,
-  -1290,
-  -250,
-  578,
-  783,
-  449,
-  -53,
-  -387,
-  -434,
-  -287,
-  -108,
-  -2,
-  28,
-  0
-};
 
 
 void setup() {
-  setI2SFreq(11000);
+  setI2SFreq(sample_rate);
 
   // Audio connections require memory to work.  For more
   // detailed information, see the MemoryAndCpuUsage example
@@ -99,16 +50,13 @@ void setup() {
   // Enable the audio shield, select input, and enable output
   sgtl5000_1.enable();
   sgtl5000_1.inputSelect(AUDIO_INPUT_MIC);
-  sgtl5000_1.micGain(8); //16 is good
+  sgtl5000_1.micGain(20); //16 is good
   //sgtl5000_1.lineInLevel(0);
   sgtl5000_1.volume(.5);
 
-  //fir1.begin(filt_coeff, TAP_NUM);
-  notefreq1.begin(1);
-
   amp1.gain(1);
 
-  fft1024_1.windowFunction(AudioWindowHanning1024);
+  fft1024.windowFunction(AudioWindowHanning1024);
 
   pinMode(PIN_A8, OUTPUT);
   digitalWrite(PIN_A8, LOW); 
@@ -131,15 +79,12 @@ void loop() {
   // }
 
 
-  if(fft1024_1.available())
+  if(fft1024.available())
   {
-
-
-
-    // max speed fft bin: 154*(44100/1024)/44 = 150,732 km/h
+    // output spectrum
     for(i = 0; i <= 511; i++)
     {
-      Serial.print((int16_t)(fft1024_1.read(i) * 1000));
+      Serial.print((int16_t)(fft1024.read(i) * 1000));
 
 
       //Serial.write((byte*)&test, sizeof(test));
@@ -149,25 +94,29 @@ void loop() {
 
       Serial.print(",");
     }
-
-    if(notefreq1.available()){
-      float speed = notefreq1.read()/44;
-      //float prob = notefreq1.probability();
-      //if(speed>5.0){
-        //Serial.printf("speed: %3.2f | Probability: %.2f\n", speed, prob);
-        Serial.print(speed);
-      //}
-
-    }else{
-      Serial.print("0");
+
+    // detect highest frequency
+    max_amplitude = 0;
+    max_freq_Index = 0;
+
+    for(i = 5; i < 512; i++) {    
+      if ((fft1024.output[i] > 20) & (fft1024.output[i] > max_amplitude)) {
+        max_amplitude = fft1024.output[i];        //remember highest amplitude
+        max_freq_Index = i;                    //remember frequency index
+      }
     }
+    Serial.print(max_freq_Index*speed_conversion);
     Serial.print(",");
 
+
+    // detect clipping / overall loudeness
     if(peak1.available()){
       Serial.print(peak1.read());
     }else{
       Serial.print("");
     }
+
+
 
     Serial.println("");
   }