diff --git a/Display.py b/Display.py new file mode 100644 index 0000000..c089ff0 --- /dev/null +++ b/Display.py @@ -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() diff --git a/FMCW_Radar/FMCW_Radar.ino b/FMCW_Radar/FMCW_Radar.ino new file mode 100644 index 0000000..aa86269 --- /dev/null +++ b/FMCW_Radar/FMCW_Radar.ino @@ -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 FFT = ArduinoFFT(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; iDACC_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; + } + + } +} \ No newline at end of file