Tried to add an ssb filter. Needs work
This commit is contained in:
parent
ff02f0e96f
commit
83cf07b67f
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -0,0 +1,98 @@
|
|||
use num::complex::Complex;
|
||||
|
||||
pub struct FirFilter<const LEN: usize> {
|
||||
state: [Complex<f32>; LEN],
|
||||
coeffs: [Complex<f32>; LEN],
|
||||
pos: usize,
|
||||
}
|
||||
|
||||
impl<const LEN: usize> FirFilter<LEN> {
|
||||
pub fn new(coeffs: [Complex<f32>; LEN]) -> FirFilter<LEN> {
|
||||
FirFilter {
|
||||
state: [Complex::new(0.0, 0.0); LEN],
|
||||
coeffs: coeffs,
|
||||
pos: 0,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn compute(&mut self, sample: Complex<f32>) -> Complex<f32> {
|
||||
self.state[self.pos] = sample;
|
||||
self.pos = (self.pos + 1) % LEN;
|
||||
|
||||
let mut result = Complex::<f32>::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),
|
||||
])
|
||||
}
|
||||
55
src/main.rs
55
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<Analog, CRL, 'A', 1>,
|
||||
q_in: gpio::Pin<Analog, CRL, 'A', 0>,
|
||||
phase: f32,
|
||||
i_offset: f32,
|
||||
q_offset: f32,
|
||||
audio_pwm: AudioPwm,
|
||||
timer: CounterHz<TIM2>,
|
||||
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::<Tim3NoRemap, _, _>(
|
||||
bias_pin,
|
||||
&mut afio.mapr,
|
||||
4800.Hz(),
|
||||
&clocks,
|
||||
);
|
||||
let mut bias_pwm =
|
||||
cx.device
|
||||
.TIM3
|
||||
.pwm_hz::<Tim3NoRemap, _, _>(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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue