fartrx-firmware/src/main.rs

#![no_main]
#![no_std]
#![feature(type_alias_impl_trait)]
use defmt_rtt as _; // global logger

use panic_probe as _;
use stm32f4xx_hal as _;

// same panicking *behavior* as `panic-probe` but doesn't print a panic message
// this prevents the panic message being printed *twice* when `defmt::panic` is invoked
#[defmt::panic_handler]
fn panic() -> ! {
    cortex_m::asm::udf()
}

use rtic::app;

mod filters;
mod si5153;

#[app(device = stm32f4xx_hal::pac, peripherals = true, dispatchers = [SPI3])]
mod app {

    use core::mem;

    use num::Complex;
    use stm32f4xx_hal::{
        adc::{
            self,
            config::{
                AdcConfig, Continuous, Dma, ExternalTrigger, SampleTime, Scan, Sequence,
                TriggerMode,
            },
            Adc,
        },
        dma::{
            self, config::DmaConfig, PeripheralToMemory, Stream0, Stream7, StreamsTuple, Transfer,
        },
        gpio::{self, gpioa, gpioc, Analog, Output, PushPull},
        i2c::{self, I2c},
        pac::{ADC1, DMA1, DMA2, I2C1, SPI1, TIM2, TIM4},
        prelude::*,
        qei,
        spi::{self, Spi},
        timer::{
            self, Channel, Channel1, Channel3, ChannelBuilder, CounterHz, Event, PwmHz, CCR, CCR3,
        },
    };

    use cortex_m::{asm, singleton};

    use microfft::complex::cfft_128;
    use num_traits::{float::Float, Pow};

    use crate::filters;
    use crate::si5153;

    use embedded_graphics::mono_font::MonoTextStyle;
    use embedded_graphics::pixelcolor::Rgb565;
    use embedded_graphics::prelude::*;
    use embedded_graphics::primitives::rectangle::Rectangle;
    use embedded_graphics::primitives::{Line, PrimitiveStyle};
    use embedded_graphics::text::Text;
    use profont::PROFONT_14_POINT;

    use core::fmt::Write;

    use st7735_lcd::{Orientation, ST7735};

    type AudioPwm = PwmHz<TIM4, ChannelBuilder<TIM4, 2>>;

    #[local]
    struct Local {
        pll: si5153::Si5153<I2c<I2C1>>,
        i2c: I2c<I2C1>,
        board_led: gpioc::PC13<Output<PushPull>>,
        rx_en: gpioa::PA7<Output<PushPull>>,
        //mic_in: gpio::Pin<'A', 4, Analog>,
        i_in: gpio::Pin<'A', 2, Analog>,
        q_in: gpio::Pin<'A', 3, Analog>,
        i_offset: f32,
        q_offset: f32,
        audio_pwm: AudioPwm,
        usb_filter: filters::FirFilter<63>,
        adc_transfer:
            Transfer<Stream0<DMA2>, 0, Adc<ADC1>, PeripheralToMemory, &'static mut [u16; 256]>,
        iq_buffer: Option<&'static mut [u16; 256]>,

        pwm_transfer: Transfer<
            Stream7<DMA1>,
            2,
            CCR<stm32f4xx_hal::pac::TIM4, 2>,
            stm32f4xx_hal::dma::MemoryToPeripheral,
            &'static mut [u16; 128],
        >,
        audio_max_duty: u16,

        disp_led: gpioa::PA10<Output<PushPull>>,
        disp_cs: gpioa::PA15<Output<PushPull>>,
        disp: ST7735<Spi<SPI1>, gpio::Pin<'A', 12, Output>, gpio::Pin<'A', 11, Output>>,
        row_pos: u16,
        row_buffer: [Complex<f32>; 128],
        row_buffer_count: usize,
        max_mag: f32,

        encoder: qei::Qei<TIM2>,
        last_encoder_pos: u32,
        carrier_freq: u32,
    }

    #[shared]
    struct Shared {
        audio_buffer: Option<&'static mut [u16; 128]>,
    }

    #[init]
    fn init(cx: init::Context) -> (Shared, Local) {
        let rcc = cx.device.RCC.constrain();

        // Freeze the configuration of all the clocks in the system and store the frozen frequencies in
        // `clocks`
        let clocks = rcc
            .cfgr
            .use_hse(25.MHz())
            .require_pll48clk()
            .sysclk(84.MHz())
            .hclk(84.MHz())
            .pclk1(42.MHz())
            .pclk2(84.MHz())
            .freeze();

        defmt::info!("Clock Setup done");

        // Acquire the GPIOC peripheral
        let gpioa = cx.device.GPIOA.split();
        let gpiob = cx.device.GPIOB.split();
        let gpioc = cx.device.GPIOC.split();

        let board_led = gpioc.pc13.into_push_pull_output();

        let enc_a = gpioa.pa0.into_input();
        let enc_b = gpioa.pa1.into_input();

        let encoder = qei::Qei::new(cx.device.TIM2, (enc_a, enc_b));

        defmt::info!("Encoder Setup done");

        let scl = gpiob.pb6.into_alternate_open_drain();
        let sda = gpiob.pb7.into_alternate_open_drain();
        let mut i2c = i2c::I2c::new(
            cx.device.I2C1,
            (scl, sda),
            i2c::Mode::Standard {
                frequency: 400.kHz(),
            },
            &clocks,
        );

        let mut pll = si5153::Si5153::new(&i2c);

        /*
        let phase = 100;
        let freq = 7_100_000;
        */

        let phase = 100;
        let freq = 7_100_000;

        pll.init(&mut i2c, 25_000_000, freq * phase, freq * phase);
        pll.set_ms_source(&mut i2c, si5153::Multisynth::MS0, si5153::PLL::A);
        pll.set_ms_source(&mut i2c, si5153::Multisynth::MS1, si5153::PLL::A);
        pll.set_ms_source(&mut i2c, si5153::Multisynth::MS2, si5153::PLL::B);

        pll.set_ms_freq(&mut i2c, si5153::Multisynth::MS0, freq);
        pll.set_ms_phase(&mut i2c, si5153::Multisynth::MS0, 0);
        pll.enable_ms_output(&mut i2c, si5153::Multisynth::MS0);

        pll.set_ms_freq(&mut i2c, si5153::Multisynth::MS1, freq);
        pll.set_ms_phase(&mut i2c, si5153::Multisynth::MS1, 100);
        pll.enable_ms_output(&mut i2c, si5153::Multisynth::MS1);

        defmt::info!("PLL chip setup done");

        let mut disp_led = gpioa.pa10.into_push_pull_output();
        disp_led.set_high();
        let mut disp_cs = gpioa.pa15.into_push_pull_output();
        disp_cs.set_low();

        let disp_rst = gpioa.pa11.into_push_pull_output();
        let disp_dc = gpioa.pa12.into_push_pull_output();

        let disp_sck = gpiob.pb3.into_alternate();
        let disp_mosi = gpiob.pb5.into_alternate();
        let spi1 = Spi::new(
            cx.device.SPI1,
            (disp_sck, spi::NoMiso::new(), disp_mosi),
            spi::Mode {
                polarity: spi::Polarity::IdleLow,
                phase: spi::Phase::CaptureOnFirstTransition,
            },
            16.MHz(),
            &clocks,
        );

        let mut disp = ST7735::new(spi1, disp_dc, disp_rst, true, false, 160, 128);

        let mut delay = cx.core.SYST.delay(&clocks);

        disp.init(&mut delay).unwrap();
        disp.set_orientation(&Orientation::Landscape).unwrap();
        disp.clear(Rgb565::BLACK).unwrap();

        defmt::info!("Display setup done");

        let i_in = gpioa.pa2.into_analog();
        let q_in = gpioa.pa3.into_analog();

        let adc_config = AdcConfig::default()
            .dma(Dma::Continuous)
            .external_trigger(TriggerMode::RisingEdge, ExternalTrigger::Tim_1_cc_1)
            .scan(Scan::Enabled);
        let mut adc1 = adc::Adc::adc1(cx.device.ADC1, true, adc_config);
        adc1.configure_channel(&i_in, Sequence::One, SampleTime::Cycles_480);
        adc1.configure_channel(&q_in, Sequence::Two, SampleTime::Cycles_480);

        let pa8 = Channel1::new(gpioa.pa8);
        let mut sample_pwm = cx.device.TIM1.pwm_hz(pa8, 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 mic_in = gpioa.pa4.into_analog()

        defmt::info!("ADC Setup done");

        let iq_buff1 = singleton!(: [u16; 256] = [0; 256]).unwrap();
        let iq_buff2 = singleton!(: [u16; 256] = [0; 256]).unwrap();

        let dma2 = StreamsTuple::new(cx.device.DMA2);

        let config = DmaConfig::default()
            .transfer_complete_interrupt(true)
            .memory_increment(true)
            .double_buffer(false);

        let mut adc_transfer =
            Transfer::init_peripheral_to_memory(dma2.0, adc1, iq_buff1, None, config);

        adc_transfer.start(|_| {});

        defmt::info!("ADC DMA Setup done");

        let ccr3_tim4: CCR3<TIM4> = unsafe { mem::transmute_copy(&cx.device.TIM4) };

        let audio_out = Channel3::new(gpiob.pb8);
        let mut audio_pwm = cx.device.TIM4.pwm_hz(audio_out, 8.kHz(), &clocks);
        audio_pwm.enable(Channel::C3);
        audio_pwm.set_duty(Channel::C3, audio_pwm.get_max_duty() / 2);
        let audio_max_duty = audio_pwm.get_max_duty();

        defmt::info!("Max duty: {}", audio_pwm.get_max_duty());

        unsafe {
            (*TIM4::ptr()).dier.modify(|_, w| {
                w.tde().set_bit(); // enable DMA trigger
                w.cc3de().set_bit(); // dma on Update
                w
            });
        };

        let audio_buff1 = singleton!(: [u16; 128] = [100; 128]).unwrap();
        let audio_buff2 = singleton!(: [u16; 128] = [100; 128]).unwrap();

        let dma1 = StreamsTuple::new(cx.device.DMA1);

        let config = DmaConfig::default()
            .transfer_complete_interrupt(true)
            .memory_increment(true)
            .double_buffer(false);

        let mut pwm_transfer =
            Transfer::init_memory_to_peripheral(dma1.7, ccr3_tim4, audio_buff1, None, config);

        pwm_transfer.start(|_| {});

        defmt::info!("PWM DMA Setup done");

        let mut rx_en = gpioa.pa7.into_push_pull_output();
        rx_en.set_high();

        let bias_pin = Channel1::new(gpioa.pa6);
        let _bias_pwm = cx.device.TIM3.pwm_hz(bias_pin, 64.kHz(), &clocks);

        (
            Shared {
                audio_buffer: Some(audio_buff2),
            },
            Local {
                i2c,
                pll,
                board_led,
                rx_en,
                //mic_in,
                i_in,
                q_in,
                i_offset: 2048.0,
                q_offset: 2048.0,
                audio_pwm,
                usb_filter: filters::usb_firfilter(),
                adc_transfer,
                iq_buffer: Some(iq_buff2),

                pwm_transfer,
                audio_max_duty,

                disp_led,
                disp_cs,
                disp,
                row_pos: 0,
                row_buffer: [Complex::<f32>::new(0.0, 0.0); 128],
                row_buffer_count: 0,
                max_mag: 0.0,

                encoder,
                last_encoder_pos: 0,
                carrier_freq: 7_100_000,
            },
        )
    }

    #[task(priority = 0, local = [disp, row_pos, row_buffer, row_buffer_count, encoder, last_encoder_pos, carrier_freq, pll, i2c])]
    async fn update_display(cx: update_display::Context, mut row: [Complex<f32>; 128]) {
        let buffers_per_row = 4;

        let row = cfft_128(&mut row);

        for idx in 0..row.len() {
            cx.local.row_buffer[idx] += row[idx] / buffers_per_row as f32;
        }

        *cx.local.row_buffer_count += 1;

        if *cx.local.row_buffer_count > buffers_per_row {
            *cx.local.row_buffer_count = 0;

            let gradient = colorous::TURBO;

            for idx in 0..128 {
                let intens: f32 =
                    cx.local.row_buffer[idx].re.pow(2) + cx.local.row_buffer[idx].im.pow(2);

                let log_intens = intens.log10() / 2.0 * 10.0;
                let norm_intens = (log_intens + 18.0) / 18.0;

                let color = gradient.eval_continuous(norm_intens as f64);

                let x = if idx < 64 { 64 + idx } else { idx - 64 };
                Pixel(
                    Point::new(32 + x as i32, 28 + *cx.local.row_pos as i32),
                    Rgb565::new(color.r >> 3, color.g >> 2, color.b >> 3),
                )
                .draw(cx.local.disp)
                .unwrap();

                cx.local.row_buffer[idx] = Complex::new(0.0, 0.0);
            }

            *cx.local.row_pos = (*cx.local.row_pos + 1) % 100;
            Line::new(
                Point::new(32, 28 + *cx.local.row_pos as i32),
                Point::new(159, 28 + *cx.local.row_pos as i32),
            )
            .into_styled(PrimitiveStyle::with_stroke(Rgb565::BLACK, 1))
            .draw(cx.local.disp)
            .unwrap();

            defmt::info!("Position is {}", cx.local.row_pos);
        }

        let encoder_pos = cx.local.encoder.count();
        let diff = encoder_pos.wrapping_sub(*cx.local.last_encoder_pos) as i32 / 4;

        if diff >= 1 || diff <= -1 {
            *cx.local.carrier_freq = (*cx.local.carrier_freq as i32 + diff * 100) as u32;
            cx.local
                .pll
                .set_pll_freq(cx.local.i2c, si5153::PLL::A, *cx.local.carrier_freq * 100);
            cx.local.pll.set_ms_freq(
                cx.local.i2c,
                si5153::Multisynth::MS0,
                *cx.local.carrier_freq,
            );
            cx.local.pll.set_ms_freq(
                cx.local.i2c,
                si5153::Multisynth::MS1,
                *cx.local.carrier_freq,
            );

            Rectangle::new(Point::new(0, 0), Size::new(160, 28))
                .into_styled(PrimitiveStyle::with_fill(Rgb565::BLACK))
                .draw(cx.local.disp)
                .unwrap();

            let mut freq_txt = arrayvec::ArrayString::<11>::new();
            write!(freq_txt, "{}", cx.local.carrier_freq).unwrap();
            let text_style = MonoTextStyle::new(&PROFONT_14_POINT, Rgb565::WHITE);
            Text::new(&freq_txt, Point::new(8, 16), text_style)
                .draw(cx.local.disp)
                .unwrap();

            *cx.local.last_encoder_pos = encoder_pos;
        }
        defmt::info!("Carrier freq is  {}", cx.local.carrier_freq);
    }

    #[task(binds = DMA2_STREAM0, local = [adc_transfer, iq_buffer, board_led, i_offset, q_offset, usb_filter, audio_max_duty], shared = [audio_buffer])]
    fn dma2_stream0(mut cx: dma2_stream0::Context) {
        let (buffer, _) = cx
            .local
            .adc_transfer
            .next_transfer(cx.local.iq_buffer.take().unwrap())
            .unwrap();

        defmt::info!("ADC transfer complete");
        cx.local.board_led.toggle();

        let mut samples = [Complex::<f32>::default(); 128];
        for idx in 0..buffer.len() / 2 {
            let q_raw = buffer[idx * 2];
            let i_raw = buffer[idx * 2 + 1];

            *cx.local.i_offset = 0.95 * *cx.local.i_offset + 0.05 * (i_raw as f32);
            *cx.local.q_offset = 0.95 * *cx.local.q_offset + 0.05 * (q_raw as f32);

            let i_sample = (i_raw as f32) - *cx.local.i_offset;
            let q_sample = (q_raw as f32) - *cx.local.q_offset;

            samples[idx] = Complex::new(q_sample as f32 / 4096.0, i_sample as f32 / 4096.0);
        }

        let mut fft_input = [Complex::<f32>::default(); 128];

        let usb_filter = cx.local.usb_filter;
        let audio_max_duty = *cx.local.audio_max_duty;

        cx.shared.audio_buffer.lock(|buffer| {
            let audio_buffer = buffer.take().unwrap();
            for idx in 0..samples.len() {
                //let filtered = usb_filter.compute(samples[idx]);
                let filtered = usb_filter.compute(Complex::new(samples[idx].im, samples[idx].re));
                fft_input[idx] = samples[idx];

                audio_buffer[idx] = ((filtered.re * (audio_max_duty as f32)) * 3.0f32) as u16;
            }

            *buffer = Some(audio_buffer);
        });

        match update_display::spawn(fft_input) {
            Ok(_) => {}
            Err(_) => defmt::warn!("Skipping display update."),
        }
        *cx.local.iq_buffer = Some(buffer);
    }

    #[task(binds = DMA1_STREAM7, local = [pwm_transfer], shared = [audio_buffer])]
    fn dma1_stream7(mut cx: dma1_stream7::Context) {
        let pwm_transfer = cx.local.pwm_transfer;
        cx.shared.audio_buffer.lock(|next_buffer| {
            let (last_buffer, _) = pwm_transfer
                .next_transfer(next_buffer.take().unwrap())
                .unwrap();

            defmt::info!("PWM transfer complete");

            *next_buffer = Some(last_buffer);
        });
    }
}