From 83cf07b67fb294621b83fd6e89145a9661b2675f Mon Sep 17 00:00:00 2001 From: Sebastian Date: Sat, 14 Jan 2023 17:31:17 +0100 Subject: [PATCH] Tried to add an ssb filter. Needs work --- Cargo.toml | 1 + src/filters.rs | 98 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/main.rs | 55 +++++++++++++++++----------- ssb_filter.py | 37 +++++++++++++++++++ 4 files changed, 171 insertions(+), 20 deletions(-) create mode 100644 src/filters.rs create mode 100644 ssb_filter.py diff --git a/Cargo.toml b/Cargo.toml index c77a2cc..b09ce1b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,6 +17,7 @@ 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} [features] diff --git a/src/filters.rs b/src/filters.rs new file mode 100644 index 0000000..0c673b7 --- /dev/null +++ b/src/filters.rs @@ -0,0 +1,98 @@ +use num::complex::Complex; + +pub struct FirFilter { + state: [Complex; LEN], + coeffs: [Complex; LEN], + pos: usize, +} + +impl FirFilter { + pub fn new(coeffs: [Complex; LEN]) -> FirFilter { + FirFilter { + state: [Complex::new(0.0, 0.0); LEN], + coeffs: coeffs, + pos: 0, + } + } + + pub fn compute(&mut self, sample: Complex) -> Complex { + self.state[self.pos] = sample; + self.pos = (self.pos + 1) % LEN; + + let mut result = Complex::::new(0.0, 0.0); + + for i in 0..LEN { + result += self.state[(self.pos + i) % LEN] * self.coeffs[i]; + } + + result + } +} + +pub fn usb_firfilter() -> FirFilter<63> { + FirFilter::new([ + Complex::new(-7.76328407e-03, 0.00000000e+00), + Complex::new(-1.76283137e-04, -4.92678363e-04), + Complex::new(-6.25577634e-03, 5.13398296e-03), + Complex::new(-6.39279452e-03, -3.83169357e-03), + Complex::new(-5.03930054e-04, 2.53342746e-03), + Complex::new(-1.03144816e-02, 1.53000882e-03), + Complex::new(-2.84091140e-03, -5.31497140e-03), + Complex::new(-4.30880421e-03, 4.75403284e-03), + Complex::new(-1.12464745e-02, -4.65844227e-03), + Complex::new(1.60156679e-04, -3.26006409e-03), + Complex::new(-1.03362126e-02, 3.13545581e-03), + Complex::new(-7.69974139e-03, -1.03818994e-02), + Complex::new(-5.76821480e-04, 8.63274351e-04), + Complex::new(-1.50337277e-02, -3.76575276e-03), + Complex::new(-1.19695644e-03, -1.21529027e-02), + Complex::new(-6.42978800e-03, 3.04106324e-03), + Complex::new(-1.42967043e-02, -1.42967043e-02), + Complex::new(4.02680910e-03, -8.51397251e-03), + Complex::new(-1.52764230e-02, -1.50459634e-03), + Complex::new(-5.98961717e-03, -2.39118921e-02), + Complex::new(2.95727524e-03, -1.97598814e-03), + Complex::new(-2.14271696e-02, -1.58914720e-02), + Complex::new(8.26910313e-03, -2.72595798e-02), + Complex::new(-7.54577227e-03, 3.70700021e-04), + Complex::new(-1.67955192e-02, -4.05479701e-02), + Complex::new(2.33021328e-02, -2.11198221e-02), + Complex::new(-2.74472271e-02, -1.46708486e-02), + Complex::new(1.15826806e-02, -7.80840900e-02), + Complex::new(3.26800996e-02, -6.50047599e-03), + Complex::new(-6.05385232e-02, -1.01002424e-01), + Complex::new(1.84598172e-01, -2.24933523e-01), + Complex::new(3.45823071e-01, 1.23737473e-01), + Complex::new(-7.14505905e-17, 2.90983805e-01), + Complex::new(-1.10872171e-01, 3.96706970e-02), + Complex::new(2.11382003e-02, 2.57569716e-02), + Complex::new(-4.05824891e-02, 6.77077926e-02), + Complex::new(-3.05240813e-02, -6.07161727e-03), + Complex::new(4.61451895e-03, 3.11085599e-02), + Complex::new(-3.87064718e-02, 2.06890402e-02), + Complex::new(-5.59779124e-03, -5.07354224e-03), + Complex::new(-1.09011912e-02, 2.63178036e-02), + Complex::new(-2.66448692e-02, -1.30897849e-03), + Complex::new(1.03245107e-03, 3.40353504e-03), + Complex::new(-1.97995805e-02, 1.46843697e-02), + Complex::new(-1.27633405e-02, -8.52819147e-03), + Complex::new(-2.28844145e-03, 9.13597039e-03), + Complex::new(-2.01212351e-02, 1.98176868e-03), + Complex::new(-3.04106324e-03, -6.42978800e-03), + Complex::new(-8.63497967e-03, 8.63497967e-03), + Complex::new(-1.40101969e-02, -6.62632965e-03), + Complex::new(1.01766417e-04, -1.03325177e-03), + Complex::new(-1.25381879e-02, 3.14065256e-03), + Complex::new(-6.00088826e-03, -8.98096395e-03), + Complex::new(-1.94435998e-03, 2.62166594e-03), + Complex::new(-1.16489269e-02, -3.53366333e-03), + Complex::new(-3.14824186e-04, -6.40839353e-03), + Complex::new(5.56783637e-03, 2.30627334e-03), + Complex::new(-7.00257479e-03, -7.72615067e-03), + Complex::new(1.21764617e-03, -2.27805575e-03), + Complex::new(-7.37249766e-03, -1.09360672e-03), + Complex::new(-1.57881619e-03, -7.93724499e-03), + Complex::new(-4.48820552e-04, 2.69012686e-04), + Complex::new(-6.00109974e-03, -4.92497528e-03), + ]) +} diff --git a/src/main.rs b/src/main.rs index 48e6ee6..badd42f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -14,11 +14,13 @@ fn panic() -> ! { use rtic::app; +mod filters; mod si5153; #[app(device = stm32f1xx_hal::pac, peripherals = true, dispatchers = [SPI3])] mod app { + use num::Complex; use stm32f1xx_hal::{ adc, gpio::{ @@ -39,6 +41,7 @@ mod app { use arrayvec::ArrayString; use num_traits::float::Float; + use crate::filters; use crate::si5153; type AppI2C1 = BlockingI2c< @@ -73,8 +76,11 @@ mod app { i_in: gpio::Pin, q_in: gpio::Pin, phase: f32, + i_offset: f32, + q_offset: f32, audio_pwm: AudioPwm, timer: CounterHz, + usb_filter: filters::FirFilter<63>, } #[init] @@ -127,15 +133,13 @@ mod app { 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, 100); + 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, 0); + pll.set_ms_phase(&mut i2c, si5153::Multisynth::MS1, 100); pll.enable_ms_output(&mut i2c, si5153::Multisynth::MS1); - pll.pll_reset(&mut i2c); - let adc1 = adc::Adc::adc1(cx.device.ADC1, clocks); let mic_in = gpioa.pa4.into_analog(&mut gpioa.crl); @@ -156,15 +160,13 @@ mod app { 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::( - bias_pin, - &mut afio.mapr, - 4800.Hz(), - &clocks, - ); + let mut bias_pwm = + cx.device + .TIM3 + .pwm_hz::(bias_pin, &mut afio.mapr, 64.kHz(), &clocks); let mut timer = timer::Timer2::new(cx.device.TIM2, &clocks).counter_hz(); - timer.start(4800.Hz()).unwrap(); + timer.start(6400.Hz()).unwrap(); // Generate an interrupt when the timer expires timer.listen(Event::Update); @@ -180,22 +182,35 @@ mod app { 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, board_led])] + #[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(mut ctx: transmit::Context) { - *ctx.local.phase += core::f32::consts::PI * 2.0 * 2000.0 / 4800.0; - if *ctx.local.phase > 2.0 * core::f32::consts::PI { - *ctx.local.phase -= 2.0 * core::f32::consts::PI; - ctx.local.board_led.toggle(); - } + ctx.local.board_led.toggle(); - //defmt::debug!("Phase: {}", ctx.local.phase); + let mut adc = ctx.local.adc1; + let mut i_in = ctx.local.i_in; + let mut 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 @@ -203,8 +218,8 @@ mod app { 2.0.powi(16) }; - let sample = (ctx.local.phase.sin() + 1.0) * max_duty / 2.0; - ctx.local.audio_pwm.set_duty(Channel::C3, sample as u16); + let output = filtered.re * max_duty; + ctx.local.audio_pwm.set_duty(Channel::C3, output as u16); ctx.local.timer.clear_interrupt(Event::Update); } diff --git a/ssb_filter.py b/ssb_filter.py new file mode 100644 index 0000000..a31fdac --- /dev/null +++ b/ssb_filter.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 + +from scipy import signal +import matplotlib.pyplot as plt +import numpy as np + + +def main(): + fs = 6400.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) + freqs, response = signal.freqz(coeffs, worN=freq_space) + response = 10 * np.log(abs(response)) + plt.plot(fs/2*freqs/(np.pi), response) + plt.grid() + plt.show() + + f0 = (1200 + 50) / fs + complex_coeffs = [] + for n in range(0, len(coeffs)): + complex_coeffs += [coeffs[n] * np.exp(1j * 2 * np.pi * f0 * n)] + complex_coeffs = np.array(complex_coeffs) + + print(complex_coeffs) + + freq_space = np.linspace(-fs/2 / (fs/2)*np.pi, fs/2 / (fs/2)*np.pi, 512) + freqs, response = signal.freqz(complex_coeffs, worN=freq_space) + response = 10 * np.log(abs(response)) + plt.plot(fs/2*freqs/(np.pi), response) + plt.grid() + plt.show() + + + +if __name__ == '__main__': + main() \ No newline at end of file