Added FFT

Cargo.toml

@@ -5,19 +5,19 @@ edition = "2018"
 version = "0.1.0"
 
 [dependencies]
-cortex-m = { version = "0.7.6", features = ["critical-section-single-core"]}
+cortex-m = { version = "0.7", features = ["critical-section-single-core"] }
-cortex-m-rt = "0.7.2"
+defmt = { version = "0.3", features = ["encoding-rzcobs"] }
-cortex-m-rtic = "1.1.3"
+defmt-brtt = { version = "0.1", default-features = false, features = ["rtt"] }
-defmt = "0.3.2"
+panic-probe = { version = "0.3", features = ["print-defmt"] }
-defmt-rtt = "0.4"
+rtic = { version = "2.0.0-alpha.1", features = [ "thumbv7-backend" ] }
-panic-probe = { version = "0.3.0", features = ["print-defmt"] }
+stm32f1xx-hal = { version = "0.10.0", features = ["stm32f103", "rt", "medium"] }
-stm32f1xx-hal = { version = "0.9.0", features = ["stm32f103", "rt", "medium"] }
 embedded-hal = {version = "0.2.3"}
 nb = "1.0.0"
 arrayvec = {version = "0.7.0", default-features = false}
 systick-monotonic = "1.0.0"
 num-traits = { version = "0.2", default-features = false, features = ["libm"] }
 num = {version = "0.4", default-features = false}
+microfft = "0.5.1"
 
 
 [features]
@@ -59,7 +59,7 @@ codegen-units = 1
 debug = 2
 debug-assertions = false # <-
 incremental = false
-lto = 'fat'
+#lto = 'fat'
 opt-level = 3            # <-
 overflow-checks = false  # <-
 
@@ -69,7 +69,7 @@ codegen-units = 1
 debug = 2
 debug-assertions = false # <-
 incremental = false
-lto = 'fat'
+#lto = 'fat'
 opt-level = 3            # <-
 overflow-checks = false  # <-
 

src/main.rs

@@ -1,6 +1,7 @@
 #![no_main]
 #![no_std]
-use defmt_rtt as _; // global logger
+#![feature(type_alias_impl_trait)]
+use defmt_brtt as _; // global logger
 
 use panic_probe as _;
 use stm32f1xx_hal as _;
@@ -20,15 +21,18 @@ mod si5153;
 #[app(device = stm32f1xx_hal::pac, peripherals = true, dispatchers = [SPI3])]
 mod app {
 
+    use cortex_m::{asm, singleton};
     use num::Complex;
     use stm32f1xx_hal::{
         adc,
+        dma::{self, CircBuffer},
         gpio::{
             self, gpioa, gpiob, gpioc, Alternate, Analog, Floating, Input, OpenDrain, Output,
-            PushPull, CRH, CRL,
+            PushPull,
         },
         i2c,
         i2c::BlockingI2c,
+        pac::adc1::cr2::EXTSEL_A,
         pac::{ADC1, I2C1, TIM2, TIM4},
         prelude::*,
         serial::{self, Config, Serial},
@@ -36,10 +40,9 @@ mod app {
         timer::{self, Channel, CounterHz, Event, Tim3NoRemap, Tim4NoRemap},
     };
 
-    use systick_monotonic::Systick;
-
     use arrayvec::ArrayString;
-    use num_traits::float::Float;
+    use microfft::complex::cfft_128;
+    use num_traits::{float::Float, Pow};
 
     use crate::filters;
     use crate::si5153;
@@ -52,15 +55,21 @@ mod app {
         ),
     >;
 
-    type AudioPwm = timer::PwmHz<
+    type AudioPwm =
-        TIM4,
+        timer::PwmHz<TIM4, Tim4NoRemap, timer::Ch<2>, gpio::Pin<'B', 8, Alternate<gpio::PushPull>>>;
-        Tim4NoRemap,
-        timer::Ch<2>,
-        gpio::Pin<Alternate<gpio::PushPull>, CRH, 'B', 8>,
-    >;
 
-    #[monotonic(binds = SysTick, default = true)]
+    pub struct AdcPins(gpio::PA0<Analog>, gpio::PA1<Analog>);
-    type MonoTimer = Systick<1_000>;
+    impl adc::SetChannels<AdcPins> for adc::Adc<ADC1> {
+        fn set_samples(&mut self) {
+            self.set_channel_sample_time(0, adc::SampleTime::T_239);
+            self.set_channel_sample_time(1, adc::SampleTime::T_239);
+        }
+        fn set_sequence(&mut self) {
+            self.set_regular_sequence(&[0, 1]);
+            // Optionally we can set continuous scan mode
+            self.set_continuous_mode(false);
+        }
+    }
 
     #[shared]
     struct Shared {}
@@ -71,20 +80,18 @@ mod app {
         i2c: AppI2C1,
         board_led: gpioc::PC13<Output<PushPull>>,
         rx_en: gpioa::PA7<Output<PushPull>>,
-        adc1: adc::Adc<ADC1>,
+        iq_buffer: dma::CircBuffer<
-        mic_in: gpio::Pin<Analog, CRL, 'A', 4>,
+            [u16; 256],
-        i_in: gpio::Pin<Analog, CRL, 'A', 1>,
+            dma::RxDma<adc::AdcPayload<ADC1, AdcPins, adc::Scan>, dma::dma1::C1>,
-        q_in: gpio::Pin<Analog, CRL, 'A', 0>,
+        >,
-        phase: f32,
         i_offset: f32,
         q_offset: f32,
         audio_pwm: AudioPwm,
-        timer: CounterHz<TIM2>,
         usb_filter: filters::FirFilter<63>,
     }
 
     #[init]
-    fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) {
+    fn init(cx: init::Context) -> (Shared, Local) {
         let mut flash = cx.device.FLASH.constrain();
         let rcc = cx.device.RCC.constrain();
 
@@ -99,8 +106,6 @@ mod app {
 
         defmt::info!("Clock Setup done");
 
-        let mono = Systick::new(cx.core.SYST, clocks.sysclk().to_Hz());
-
         let mut afio = cx.device.AFIO.constrain();
 
         // Acquire the GPIOC peripheral
@@ -140,35 +145,54 @@ mod app {
         pll.set_ms_phase(&mut i2c, si5153::Multisynth::MS1, 100);
         pll.enable_ms_output(&mut i2c, si5153::Multisynth::MS1);
 
-        let adc1 = adc::Adc::adc1(cx.device.ADC1, clocks);
+        defmt::info!("Si5153 Setup done");
-        let mic_in = gpioa.pa4.into_analog(&mut gpioa.crl);
+
+        let dma1_channels = cx.device.DMA1.split();
+        let dma1_ch1 = dma1_channels.1;
+        // Setup ADC
+        let mut adc1 = adc::Adc::adc1(cx.device.ADC1, clocks);
+        adc1.set_external_trigger(EXTSEL_A::Tim1cc1);
+
+        let pa8 = gpioa.pa8.into_alternate_push_pull(&mut gpioa.crh);
+        let mut sample_pwm = cx.device.TIM1.pwm_hz::<timer::Tim1NoRemap, _, _>(
+            pa8,
+            &mut afio.mapr,
+            8.kHz(),
+            &clocks,
+        );
+        let max_duty = sample_pwm.get_max_duty();
+        sample_pwm.set_duty(Channel::C1, max_duty / 2);
+        sample_pwm.enable(Channel::C1);
 
         let i_in = gpioa.pa1.into_analog(&mut gpioa.crl);
         let q_in = gpioa.pa0.into_analog(&mut gpioa.crl);
 
+        let adc_dma = adc1.with_scan_dma(AdcPins(q_in, i_in), dma1_ch1);
+
+        let buf = singleton!(: [[u16; 256]; 2] = [[0; 256]; 2]).unwrap();
+        let iq_buffer = adc_dma.circ_read(buf);
+
+        //let mic_in = gpioa.pa4.into_analog(&mut gpioa.crl);
+
         let audio_out = gpiob.pb8.into_alternate_push_pull(&mut gpiob.crh);
-        let mut audio_pwm = cx.device.TIM4.pwm_hz::<Tim4NoRemap, _, _>(
+        let mut audio_pwm =
-            audio_out,
+            cx.device
-            &mut afio.mapr,
+                .TIM4
-            192.kHz(),
+                .pwm_hz::<Tim4NoRemap, _, _>(audio_out, &mut afio.mapr, 8.kHz(), &clocks);
-            &clocks,
-        );
         audio_pwm.enable(Channel::C3);
         audio_pwm.set_duty(Channel::C3, 0u16);
 
         let mut rx_en = gpioa.pa7.into_push_pull_output(&mut gpioa.crl);
         rx_en.set_high();
 
+        /*
         let mut bias_pin = gpioa.pa6.into_alternate_push_pull(&mut gpioa.crl);
         let mut bias_pwm =
             cx.device
                 .TIM3
-                .pwm_hz::<Tim3NoRemap, _, _>(bias_pin, &mut afio.mapr, 64.kHz(), &clocks);
+                .pwm_hz::<Tim3NoRemap, _, _>(bias_pin, &mut afio.mapr, 64.kHz(), &clocks);*/
 
-        let mut timer = timer::Timer2::new(cx.device.TIM2, &clocks).counter_hz();
+        receiver_task::spawn().ok();
-        timer.start(6400.Hz()).unwrap();
-        // Generate an interrupt when the timer expires
-        timer.listen(Event::Update);
 
         (
             Shared {},
@@ -177,50 +201,83 @@ mod app {
                 pll,
                 board_led,
                 rx_en,
-                adc1,
+                iq_buffer,
-                mic_in,
-                i_in,
-                q_in,
-                phase: 0.0,
                 i_offset: 2048.0,
                 q_offset: 2048.0,
                 audio_pwm,
-                timer,
                 usb_filter: filters::usb_firfilter(),
             },
-            init::Monotonics(mono),
         )
     }
 
-    #[task(binds=TIM2, local=[timer, pll, i2c, adc1, mic_in, i_in, q_in, audio_pwm, phase, i_offset, q_offset, board_led, usb_filter])]
+    #[task(local=[board_led, iq_buffer, board_led, usb_filter])]
-    fn transmit(mut ctx: transmit::Context) {
+    async fn receiver_task(ctx: receiver_task::Context) {
-        ctx.local.board_led.toggle();
+        defmt::info!("Start receiver_task!");
 
-        let mut adc = ctx.local.adc1;
+        let mut i_offset = 0.0;
-        let mut i_in = ctx.local.i_in;
+        let mut q_offset = 0.0;
-        let mut q_in = ctx.local.q_in;
 
-        let i_raw: u16 = adc.read(&mut *q_in).unwrap();
+        let mut expected_half = dma::Half::First;
-        let q_raw: u16 = adc.read(&mut *i_in).unwrap();
 
-        *ctx.local.i_offset = 0.95 * *ctx.local.i_offset + 0.05 * (i_raw as f32);
+        loop {
-        *ctx.local.q_offset = 0.95 * *ctx.local.q_offset + 0.05 * (q_raw as f32);
+            while ctx.local.iq_buffer.readable_half().unwrap() != expected_half {}
+            ctx.local.board_led.set_low();
 
-        let i_sample = (i_raw as f32) - *ctx.local.i_offset;
+            let samples = ctx
-        let q_sample = (q_raw as f32) - *ctx.local.q_offset;
+                .local
+                .iq_buffer
+                .peek(|half, _| {
+                    let mut samples = [Complex::<f32>::default(); 128];
+                    for idx in 0..half.len() / 2 {
+                        let q_raw = half[idx * 2];
+                        let i_raw = half[idx * 2 + 1];
 
-        let sample = Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
+                        i_offset = 0.95 * i_offset + 0.05 * (i_raw as f32);
-        let filtered = ctx.local.usb_filter.compute(sample) * 2.0;
+                        q_offset = 0.95 * q_offset + 0.05 * (q_raw as f32);
 
-        let max_duty = if ctx.local.audio_pwm.get_max_duty() != 0 {
+                        let i_sample = (i_raw as f32) - i_offset;
-            ctx.local.audio_pwm.get_max_duty() as f32
+                        let q_sample = (q_raw as f32) - q_offset;
-        } else {
-            2.0.powi(16)
-        };
 
-        let output = filtered.re * max_duty;
+                        samples[idx] =
-        ctx.local.audio_pwm.set_duty(Channel::C3, output as u16);
+                            Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
+                    }
+                    samples
+                })
+                .unwrap();
 
-        ctx.local.timer.clear_interrupt(Event::Update);
+            let mut fft_input = [Complex::<f32>::default(); 128];
+            for idx in 0..samples.len() / 2 {
+                let _filtered = ctx.local.usb_filter.compute(samples[idx]) * 2.0;
+                fft_input[idx] = samples[idx];
+            }
+
+            let spectrum = cfft_128(&mut fft_input);
+
+            let mut max_idx: usize = 0;
+            let mut max_mag = 0.0;
+            for idx in 0..spectrum.len() {
+                let mag_cur = ((spectrum[idx].re.pow(2) + spectrum[idx].im.pow(2)) as f32).sqrt();
+
+                if mag_cur > max_mag {
+                    max_idx = idx;
+                    max_mag = mag_cur;
+                }
+            }
+            defmt::debug!(
+                "Max at {}kHz: {}",
+                max_idx as f32 * (8.0 / 128.0) - 4.0,
+                max_mag
+            );
+
+            ctx.local.board_led.set_high();
+
+            expected_half = if expected_half == dma::Half::First {
+                defmt::info!("Switching to second half.");
+                dma::Half::Second
+            } else {
+                defmt::info!("Switching to first half.");
+                dma::Half::First
+            }
+        }
     }
 }

ssb_filter.py

@@ -6,7 +6,7 @@ import numpy as np
 
 
 def main():
-    fs = 6400.0
+    fs = 8000.0
     coeffs = signal.firls(63, (0, 1150, 1200, fs/2), (1, 1, 0, 0), fs=fs)
 
     freq_space = np.linspace(-fs/2 / (fs/2)*np.pi, fs/2 / (fs/2)*np.pi, 512)