Added signal processing code
This commit is contained in:
parent
f9dcb07de5
commit
bd69ef82b9
11
Cargo.toml
11
Cargo.toml
|
@ -5,12 +5,12 @@ edition = "2018"
|
|||
version = "0.1.0"
|
||||
|
||||
[dependencies]
|
||||
cortex-m = { version = "0.7.6", features = ["critical-section-single-core"]}
|
||||
cortex-m-rt = "0.7.2"
|
||||
cortex-m-rtic = "1.1.3"
|
||||
defmt = "0.3.2"
|
||||
cortex-m = { version = "0.7", features = ["critical-section-single-core"] }
|
||||
defmt = { version = "0.3", features = ["encoding-rzcobs"] }
|
||||
defmt-brtt = { version = "0.1", default-features = false, features = ["rtt"] }
|
||||
panic-probe = { version = "0.3", features = ["print-defmt"] }
|
||||
rtic = { version = "2.0.0-alpha.1", features = [ "thumbv7-backend" ] }
|
||||
defmt-rtt = "0.4"
|
||||
panic-probe = { version = "0.3.0", features = ["print-defmt"] }
|
||||
stm32f4xx-hal = { version = "0.16.2", features = ["stm32f401"] }
|
||||
embedded-hal = {version = "0.2.3"}
|
||||
nb = "1.0.0"
|
||||
|
@ -18,6 +18,7 @@ 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]
|
||||
|
|
182
src/main.rs
182
src/main.rs
|
@ -20,31 +20,35 @@ mod si5153;
|
|||
#[app(device = stm32f4xx_hal::pac, peripherals = true, dispatchers = [SPI3])]
|
||||
mod app {
|
||||
|
||||
use embedded_hal::blocking::spi::transfer;
|
||||
use num::Complex;
|
||||
use stm32f4xx_hal::{
|
||||
adc::{self, config::AdcConfig},
|
||||
adc::{
|
||||
self,
|
||||
config::{
|
||||
AdcConfig, Continuous, Dma, ExternalTrigger, SampleTime, Scan, Sequence,
|
||||
TriggerMode,
|
||||
},
|
||||
Adc,
|
||||
},
|
||||
dma::{self, config::DmaConfig, PeripheralToMemory, Stream0, StreamsTuple, Transfer},
|
||||
gpio::{self, gpioa, gpioc, Analog, Output, PushPull},
|
||||
i2c::{self, I2c},
|
||||
pac::{ADC1, I2C1, TIM2, TIM4},
|
||||
pac::{ADC1, DMA2, I2C1, TIM2, TIM4},
|
||||
prelude::*,
|
||||
timer::{
|
||||
self, Channel, Channel1, Channel3, ChannelBuilder, CounterHz, Event, PwmHz,
|
||||
},
|
||||
timer::{self, Channel, Channel1, Channel3, ChannelBuilder, CounterHz, Event, PwmHz},
|
||||
};
|
||||
|
||||
use systick_monotonic::Systick;
|
||||
use cortex_m::{asm, singleton};
|
||||
|
||||
|
||||
use num_traits::float::Float;
|
||||
use microfft::complex::cfft_128;
|
||||
use num_traits::{float::Float, Pow};
|
||||
|
||||
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 {}
|
||||
|
||||
|
@ -54,35 +58,37 @@ mod app {
|
|||
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>,
|
||||
//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>,
|
||||
transfer:
|
||||
Transfer<Stream0<DMA2>, 0, Adc<ADC1>, PeripheralToMemory, &'static mut [u16; 256]>,
|
||||
iq_buffer: Option<&'static mut [u16; 256]>,
|
||||
}
|
||||
|
||||
#[init]
|
||||
fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) {
|
||||
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(8.MHz())
|
||||
.sysclk(72.MHz())
|
||||
.pclk1(36.MHz())
|
||||
.use_hse(25.MHz())
|
||||
.require_pll48clk()
|
||||
.sysclk(84.MHz())
|
||||
.hclk(84.MHz())
|
||||
.pclk1(42.MHz())
|
||||
.pclk2(84.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();
|
||||
|
@ -102,25 +108,61 @@ mod app {
|
|||
);
|
||||
|
||||
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.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::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);
|
||||
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();
|
||||
defmt::info!("PLL chip setup done");
|
||||
|
||||
let i_in = gpioa.pa1.into_analog();
|
||||
let q_in = gpioa.pa0.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 transfer =
|
||||
Transfer::init_peripheral_to_memory(dma2.0, adc1, iq_buff1, None, config);
|
||||
|
||||
transfer.start(|_| {});
|
||||
|
||||
defmt::info!("DMA Setup done");
|
||||
|
||||
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);
|
||||
|
@ -132,11 +174,6 @@ mod app {
|
|||
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 {
|
||||
|
@ -144,50 +181,69 @@ mod app {
|
|||
pll,
|
||||
board_led,
|
||||
rx_en,
|
||||
adc1,
|
||||
mic_in,
|
||||
//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(),
|
||||
transfer,
|
||||
iq_buffer: Some(iq_buff2),
|
||||
},
|
||||
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();
|
||||
#[task(binds = DMA2_STREAM0, local = [transfer, iq_buffer, board_led, i_offset, q_offset, usb_filter])]
|
||||
fn dma2_stream0(cx: dma2_stream0::Context) {
|
||||
let (buffer, _) = cx
|
||||
.local
|
||||
.transfer
|
||||
.next_transfer(cx.local.iq_buffer.take().unwrap())
|
||||
.unwrap();
|
||||
|
||||
let adc = ctx.local.adc1;
|
||||
let i_in = ctx.local.i_in;
|
||||
let q_in = ctx.local.q_in;
|
||||
defmt::info!("Transfer complete");
|
||||
cx.local.board_led.toggle();
|
||||
|
||||
let i_raw: u16 = adc.read(&mut *q_in).unwrap();
|
||||
let q_raw: u16 = adc.read(&mut *i_in).unwrap();
|
||||
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];
|
||||
|
||||
*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);
|
||||
*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) - *ctx.local.i_offset;
|
||||
let q_sample = (q_raw as f32) - *ctx.local.q_offset;
|
||||
let i_sample = (i_raw as f32) - *cx.local.i_offset;
|
||||
let q_sample = (q_raw as f32) - *cx.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;
|
||||
samples[idx] = Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.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 mut fft_input = [Complex::<f32>::default(); 128];
|
||||
for idx in 0..samples.len() / 2 {
|
||||
let _filtered = cx.local.usb_filter.compute(samples[idx]) * 2.0;
|
||||
fft_input[idx] = samples[idx];
|
||||
}
|
||||
|
||||
let output = filtered.re * max_duty;
|
||||
ctx.local.audio_pwm.set_duty(Channel::C3, output as u16);
|
||||
let spectrum = cfft_128(&mut fft_input);
|
||||
|
||||
ctx.local.timer.clear_interrupt(Event::Update);
|
||||
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
|
||||
);
|
||||
|
||||
*cx.local.iq_buffer = Some(buffer);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue