fartrx-firmware/src/main.rs

#![no_main]
#![no_std]
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 num::Complex;
    use stm32f4xx_hal::{
        adc::{self, config::AdcConfig},
        gpio::{self, gpioa, gpioc, Analog, Output, PushPull},
        i2c::{self, I2c},
        pac::{ADC1, I2C1, TIM2, TIM4},
        prelude::*,
        timer::{
            self, Channel, Channel1, Channel3, ChannelBuilder, CounterHz, Event, PwmHz,
        },
    };

    use systick_monotonic::Systick;

    
    use num_traits::float::Float;

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

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

    #[monotonic(binds = SysTick, default = true)]
    type MonoTimer = Systick<1_000>;

    #[shared]
    struct Shared {}

    #[local]
    struct Local {
        pll: si5153::Si5153<I2c<I2C1>>,
        i2c: I2c<I2C1>,
        board_led: gpioc::PC13<Output<PushPull>>,
        rx_en: gpioa::PA7<Output<PushPull>>,
        adc1: adc::Adc<ADC1>,
        mic_in: gpio::Pin<'A', 4, Analog>,
        i_in: gpio::Pin<'A', 1, Analog>,
        q_in: gpio::Pin<'A', 0, Analog>,
        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) {
        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(8.MHz())
            .sysclk(72.MHz())
            .pclk1(36.MHz())
            .freeze();

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

        let mono = Systick::new(cx.core.SYST, clocks.sysclk().to_Hz());

        // 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 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);
        pll.init(&mut i2c, 25000000, 800000000, 800000000);
        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, 8_000_000);
        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, 8_000_000);
        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, true, AdcConfig::default());
        let mic_in = gpioa.pa4.into_analog();

        let i_in = gpioa.pa1.into_analog();
        let q_in = gpioa.pa0.into_analog();

        let audio_out = Channel3::new(gpiob.pb8);
        let mut audio_pwm = cx.device.TIM4.pwm_hz(audio_out, 192.kHz(), &clocks);
        audio_pwm.enable(Channel::C3);
        audio_pwm.set_duty(Channel::C3, 0u16);

        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);

        let mut timer = timer::Timer2::new(cx.device.TIM2, &clocks).counter_hz();
        timer.start(6400.Hz()).unwrap();
        // Generate an interrupt when the timer expires
        timer.listen(Event::Update);

        (
            Shared {},
            Local {
                i2c,
                pll,
                board_led,
                rx_en,
                adc1,
                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])]
    fn transmit(ctx: transmit::Context) {
        ctx.local.board_led.toggle();

        let adc = ctx.local.adc1;
        let i_in = ctx.local.i_in;
        let q_in = ctx.local.q_in;

        let i_raw: u16 = adc.read(&mut *q_in).unwrap();
        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);
        *ctx.local.q_offset = 0.95 * *ctx.local.q_offset + 0.05 * (q_raw as f32);

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

        let sample = Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
        let filtered = ctx.local.usb_filter.compute(sample) * 2.0;

        let max_duty = if ctx.local.audio_pwm.get_max_duty() != 0 {
            ctx.local.audio_pwm.get_max_duty() as f32
        } else {
            2.0.powi(16)
        };

        let output = filtered.re * max_duty;
        ctx.local.audio_pwm.set_duty(Channel::C3, output as u16);

        ctx.local.timer.clear_interrupt(Event::Update);
    }
}