feat: osnovna arduino skica + display skripta za prvih 100 vzorcev

2026-01-24 20:14:25 +01:00
parent 1117213f82
commit 97728bd829
2 changed files with 291 additions and 0 deletions
@@ -0,0 +1,92 @@
+import serial
+import struct
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+import sys
+
+# --- KONFIGURACIJA---
+SERIAL_PORT = '/dev/ttyACM0'   # Preveri svoj port
+BAUD_RATE = 115200     
+FFT_SIZE = 1024
+BIN_COUNT = FFT_SIZE // 2  # 512 Bins
+DISPLAY_BINS = 100  # Only display first 100 bins
+SWEEP_SIZE = 2
+BYTES_PER_FRAME = BIN_COUNT * 4 + SWEEP_SIZE
+WATERFALL_HISTORY = 100  # Number of frames to display in waterfall
+
+try:
+    ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=2)
+    ser.reset_input_buffer()
+    print("Povezava z Arduino Due vzpostavljena")
+except Exception as e:
+    print(f"Error: {e}")
+    sys.exit()
+
+# Setup Plot with two subplots
+fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 10))
+
+x_freq = np.zeros(DISPLAY_BINS)
+y_data = np.zeros(DISPLAY_BINS)
+
+# FFT plot (top)
+bar_plot, = ax1.plot(x_freq, y_data, color="#ff0000", lw=1)
+ax1.set_xlabel('Razdalja [m]')
+ax1.set_ylabel('Amplituda')
+
+# Waterfall plot (bottom)
+waterfall_data = np.zeros((WATERFALL_HISTORY, DISPLAY_BINS))
+waterfall_img = ax2.imshow(waterfall_data, aspect='auto', cmap='viridis', 
+                           interpolation='nearest', origin='upper')
+ax2.set_xlabel('Razdalja [m]')
+ax2.set_ylabel('Čas (frame)')
+ax2.set_title('Waterfall prikaz')
+cbar = plt.colorbar(waterfall_img, ax=ax2, label='Amplituda')
+
+def update(frame):
+    global waterfall_data
+    
+    if ser.in_waiting >= BYTES_PER_FRAME:
+        raw_data = ser.read(BYTES_PER_FRAME)
+        
+        try:
+            fm_sweep = struct.unpack(f'H', raw_data[:2])
+
+            x_dist = np.linspace(0, 1024*3e8/100e6/fm_sweep[0], BIN_COUNT)
+            x_dist_display = x_dist[:DISPLAY_BINS]  # Only first 100 bins
+            ax1.set_title(f'FFT prikaz radarskega signala FM={fm_sweep[0]}')
+            bar_plot.set_xdata(x_dist_display)
+            ax1.set_xlim(x_dist_display[2], x_dist_display[-1]) # Skip DC
+
+            fft_data = struct.unpack(f'{BIN_COUNT}f', raw_data[2:])
+            fft_data_display = fft_data[:DISPLAY_BINS]  # Only use first 100 bins
+
+            # Update FFT plot
+            bar_plot.set_ydata(fft_data_display)
+
+            # Auto-scale FFT plot
+            peak = np.max(fft_data_display[2:]) 
+            if peak > ax1.get_ylim()[1]:
+                ax1.set_ylim(0, peak * 1.2)
+            elif peak < ax1.get_ylim()[1] * 0.5 and peak > 10:
+                ax1.set_ylim(0, peak * 1.5)
+            
+            # Update waterfall (scroll up and add new data at bottom)
+            waterfall_data = np.roll(waterfall_data, -1, axis=0)
+            waterfall_data[-1, :] = fft_data[:DISPLAY_BINS]
+            
+            waterfall_img.set_data(waterfall_data)
+            waterfall_img.set_clim(vmin=np.min(waterfall_data), vmax=np.max(waterfall_data))
+            
+            # Update waterfall x-axis to match distance
+            waterfall_img.set_extent([x_dist_display[0], x_dist_display[-1], WATERFALL_HISTORY, 0])
+                
+        except Exception as e:
+            print(f"Frame Error: {e}")
+            ser.reset_input_buffer()
+
+    return bar_plot, waterfall_img
+
+ani = FuncAnimation(fig, update, interval=0, blit=True)
+plt.tight_layout()
+plt.show()
@@ -0,0 +1,199 @@
+// ARDUINO DUE: ROBUSTNI DMA RADAR (ODSTRANITEV DC + PRILAGOJEN ADC)
+// Strojna oprema: Signal na pinu A1 (ADC kanal 6)
+
+#include "arduinoFFT.h"
+
+// --- KONFIGURACIJA ---
+#define SAMPLES 1024             // Število vzorcev
+#define SAMPLING_FREQUENCY 2048  // Frekvenca vzorčenja v Hz
+
+
+// --- TABELA ---
+const uint16_t FM_FREQ=2; // 30 preletov/sekundo
+const uint16_t FM_SAMPLES=4096; // 12 bitov
+uint16_t FM_table_pos[FM_SAMPLES];
+uint16_t FM_table_neg[FM_SAMPLES];
+
+int DAC_working=0;
+// --- MEDPOMNILNIKI (BUFFERS) ---
+volatile uint16_t bufferA[SAMPLES];
+volatile uint16_t bufferB[SAMPLES];
+volatile bool bufferA_Ready = false; 
+volatile bool bufferB_Ready = false;
+
+float vReal[SAMPLES];
+float vImag[SAMPLES];
+
+ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal, vImag, SAMPLES, SAMPLING_FREQUENCY);
+
+void setup() {
+  // 1. Postavimo serijsko komunikacijo
+  SerialUSB.begin(115200);
+  while (!SerialUSB); // Počakaj na povezavo z računalnikom
+
+  // 2. Poračunamo wavetable za FM signal
+  for (uint16_t i=0; i<FM_SAMPLES; i++) {
+    FM_table_pos[i] = i;      // Napolnimo tabelo z linearnim sweepom navzgor
+    FM_table_neg[i] = 4095-i; // Napolnimo tabelo z linearnim sweepom navzdol
+  }
+
+  // 3. DACC - Digitalno-Analogni pretvornik
+  // Krmiljenje VCO Tune signala
+
+  pmc_enable_periph_clk(ID_DACC); // Vklopi clock za DACC
+  
+  DACC->DACC_CR = DACC_CR_SWRST; // Resetiraj DACC
+
+
+  DACC->DACC_MR = DACC_MR_TRGSEL(2)| // Proženje na Timer Channel 1
+                  DACC_MR_WORD_HALF| // Prenos po 16-bitov naenkrat
+                  DACC_MR_STARTUP_512| // 512-bitni startup cikel
+                  DACC_MR_USER_SEL_CHANNEL0| // Izhod na DAC0
+                  DACC_MR_REFRESH(0)| // Brez refresha, ker se signal spremenja hitreje od Sample&Hold trajanja
+                  DACC_MR_TAG_DIS| // brez TAGanja, ker vedno pišemo DAC0
+                  DACC_MR_MAXS_NORMAL; // Normalni, ne Max Speed mode
+
+  DACC->DACC_CHER = DACC_CHER_CH0; // Enablaj DAC0 izhod
+  DACC->DACC_PTCR = DACC_PTCR_TXTDIS; // Disablaj DACC DMA kontroler
+  DACC->DACC_TPR = (uint32_t) FM_table_pos; // Najprej začni z pozitivnim sweepom
+  DACC->DACC_TNPR = (uint32_t) FM_table_neg; // Zatem z negativnim
+  DACC->DACC_TCR = FM_SAMPLES; // Dolžina sweep tabele
+  DACC->DACC_TNCR = FM_SAMPLES;
+  DACC->DACC_PTCR = DACC_PTCR_TXTEN; // Enablaj DACC DMA kontroler
+
+  
+  NVIC_EnableIRQ(DACC_IRQn); // Enablaj prekinitev za DACC
+  DACC->DACC_IDR = DACC_IDR_TXRDY | DACC_IDR_EOC;   // Izklopi TXRDY, EOC prekinitve
+  DACC->DACC_IER = DACC_IER_ENDTX;                  // Vklopi ENDTX prekinitev (ko DAC preleti eno tabelo)
+  
+  DACC->DACC_MR |= DACC_MR_TRGEN_EN; // Vklopi proženje DACC
+  DACC->DACC_CDR = FM_table_pos[0];  // Naloži prvo vrednost v DACC
+
+  // 4. ADC - Analogno-Digitalni pretvornik
+  // Vzorčenje povratnega signala v osnovnem pasu
+  pmc_enable_periph_clk(ID_ADC);
+  adc_init(ADC, SystemCoreClock, ADC_FREQ_MAX, ADC_STARTUP_FAST);
+
+  // TRACKTIM(15): Poveča čas vzorčenja in zadrževanja (daljše čakanje, da se napetost ustali)
+  // TRANSFER(3):  Optimizira čas prenosa med analognim in digitalnim delom
+  // TRGEN / TRGSEL: Še vedno uporablja proženje s Timerjem 0
+  ADC->ADC_MR = ADC_MR_TRGEN | 
+                ADC_MR_TRGSEL_ADC_TRIG1 | 
+                ADC_MR_TRACKTIM(15) | 
+                ADC_MR_TRANSFER(3);
+
+  ADC->ADC_CHER = 0x40; // Omogoči kanal 6 (Pin A1)
+
+  // ADC DMA (PDC)
+  ADC->ADC_PTCR = ADC_PTCR_RXTDIS;   // 
+  ADC->ADC_RPR = (uint32_t)bufferA;  // Kazalec na trenutni medpomnilnik
+  ADC->ADC_RCR = SAMPLES;            // Število vzorcev za prejem
+  ADC->ADC_RNPR = (uint32_t)bufferB; // Kazalec na naslednji medpomnilnik
+  ADC->ADC_RNCR = SAMPLES;           // Število vzorcev za naslednji prejem
+  ADC->ADC_PTCR = ADC_PTCR_RXTEN;    // Omogoči DMA prejem
+
+  // ADC DMA prekinitev (Interrupt)
+  NVIC_EnableIRQ(ADC_IRQn);
+  ADC->ADC_IER = ADC_IER_ENDRX;      // Prekinitev ob koncu prejema medpomnilnika
+
+  // 5. Timer Counterja TC0 in TC1
+  // TC0: 2048 Hz: krmili vzorčenje signala v osnovnem pasu
+  // TC1: VAR  Hz: krmili frekvenčni prelet VCO Tune signala
+  pmc_enable_periph_clk(ID_TC0); // Clock TC0
+  pmc_enable_periph_clk(ID_TC1); // Clock TC1
+
+  // Channel 0 (ADC trigger) - unchanged
+  TC_Configure(TC0, 0, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | 
+                      TC_CMR_TCCLKS_TIMER_CLOCK2 | 
+                      TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
+  const uint32_t rc_adc = 5127; // 2048 Hz
+  TC_SetRA(TC0, 0, rc_adc / 2);
+  TC_SetRC(TC0, 0, rc_adc);
+
+  // Channel 1 (DAC trigger) - use CLOCK1
+  TC_Configure(TC0, 1, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | 
+                      TC_CMR_TCCLKS_TIMER_CLOCK1 | 
+                      TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);  
+
+  const uint32_t dac_period = 84000000/2/4096/FM_FREQ;
+  TC_SetRA(TC0, 1, dac_period / 2);   
+  TC_SetRC(TC0, 1, dac_period);
+
+  // Sinhronizacija Timer bloka
+  TC0->TC_BCR = TC_BCR_SYNC;
+
+  // Zagon timerja
+  TC_Start(TC0, 0);
+  TC_Start(TC0, 1);
+
+  pinMode(2, OUTPUT);
+  digitalWrite(2, HIGH); // Vklop VCO-ja
+}
+
+void loop() {
+  if (bufferA_Ready || bufferB_Ready) {
+    // Izberi pripravljen medpomnilnik
+    volatile uint16_t* inputBuffer = bufferA_Ready ? bufferA : bufferB;
+
+    // --- KORAK 1: IZRAČUN DC ODMIKA (OFFSET) ---
+    // Seštejemo vse vzorce, da najdemo povprečno napetost
+    float dcBias = 0;
+    for (int i = 0; i < SAMPLES; i++) {
+      dcBias += inputBuffer[i];
+    }
+    dcBias = dcBias / SAMPLES;
+
+    // --- KORAK 2: POLNJENJE FFT POLJ IN ODSTRANITEV DC ---
+    // Odštevanje dcBias
+    for (int i = 0; i < SAMPLES; i++) {
+      vReal[i] = (float)inputBuffer[i] - dcBias;
+      vImag[i] = 0.0;
+    }
+
+    // --- KORAK 3: IZRAČUN FFT ---
+    FFT.windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD); // HAMMING-ovo okno
+    FFT.compute(FFT_FORWARD);
+    FFT.complexToMagnitude();
+
+    // --- KORAK 4: POŠILJANJE PODATKOV ---
+    // Pošljemo samo prvo polovico spektra (SAMPLES / 2)
+    SerialUSB.write((uint8_t*) &FM_FREQ, sizeof(uint16_t));
+    SerialUSB.write((uint8_t*)vReal, (SAMPLES / 2) * sizeof(float));
+    // Ponastavi zastavico
+    if (bufferA_Ready) bufferA_Ready = false;
+    else bufferB_Ready = false;
+
+
+  }
+}
+
+// Prekinitveni program (ISR) za ADC
+void ADC_Handler() {
+  if ((ADC->ADC_ISR & ADC_ISR_ENDRX) == ADC_ISR_ENDRX) {
+    // Preveri, kateri medpomnilnik je ADC pravkar napolnil
+    // Opomba: RPR kaže na TRENUTNEGA, kar pomeni, da je prejšnji končan
+    if (ADC->ADC_RPR == (uint32_t)bufferB) {
+      bufferA_Ready = true;               
+      ADC->ADC_RNPR = (uint32_t)bufferA;  // Pripravi Buffer A kot naslednjega
+      ADC->ADC_RNCR = SAMPLES;
+    } else {
+      bufferB_Ready = true;               
+      ADC->ADC_RNPR = (uint32_t)bufferB;  // Pripravi Buffer B kot naslednjega
+      ADC->ADC_RNCR = SAMPLES;
+    }
+  }
+}
+
+void DACC_Handler(void) {
+  uint32_t status = DACC->DACC_ISR;  // Beri status
+  if (status & DACC_ISR_ENDTX) {
+    if(DACC->DACC_TPR == (uint32_t)FM_table_pos) { // Izberemo nasprotno smer preleta, ko pridemo do konca tabele
+      DACC->DACC_TNPR = (uint32_t)FM_table_pos;
+      DACC->DACC_TNCR = FM_SAMPLES;
+    } else {
+      DACC->DACC_TNPR = (uint32_t)FM_table_pos;
+      DACC->DACC_TNCR = FM_SAMPLES;
+    }    
+    
+  }
+}