fartrx
/
fartrx-firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added signal processing code

This commit is contained in:
Sebastian 2023-07-04 18:44:12 +02:00
parent f9dcb07de5
commit bd69ef82b9
2 changed files with 125 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Cargo.toml
View File

@ -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
View File

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