Added FFT
This commit is contained in:
parent
a1fe0d2bb0
commit
97e782c9ce
|
@ -17,6 +17,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]
|
||||
|
|
59
src/main.rs
59
src/main.rs
|
@ -41,7 +41,8 @@ mod app {
|
|||
};
|
||||
|
||||
use arrayvec::ArrayString;
|
||||
use num_traits::float::Float;
|
||||
use microfft::complex::cfft_128;
|
||||
use num_traits::{float::Float, Pow};
|
||||
|
||||
use crate::filters;
|
||||
use crate::si5153;
|
||||
|
@ -209,31 +210,65 @@ mod app {
|
|||
)
|
||||
}
|
||||
|
||||
#[task(local=[board_led, iq_buffer, i_offset, q_offset, board_led, usb_filter])]
|
||||
#[task(local=[board_led, iq_buffer, board_led, usb_filter])]
|
||||
async fn receiver_task(ctx: receiver_task::Context) {
|
||||
defmt::info!("Start receiver_task!");
|
||||
|
||||
let mut i_offset = 0.0;
|
||||
let mut q_offset = 0.0;
|
||||
|
||||
let mut expected_half = dma::Half::First;
|
||||
|
||||
loop {
|
||||
while ctx.local.iq_buffer.readable_half().unwrap() != expected_half {}
|
||||
ctx.local.board_led.set_low();
|
||||
|
||||
let half = ctx.local.iq_buffer.peek(|half, _| *half).unwrap();
|
||||
let samples = ctx
|
||||
.local
|
||||
.iq_buffer
|
||||
.peek(|half, _| {
|
||||
let mut samples = [Complex::<f32>::default(); 128];
|
||||
for idx in 0..half.len() / 2 {
|
||||
let q_raw = half[idx * 2];
|
||||
let i_raw = half[idx * 2 + 1];
|
||||
|
||||
for idx in 0..half.len() / 2 {
|
||||
let q_raw = half[idx * 2];
|
||||
let i_raw = half[idx * 2 + 1];
|
||||
i_offset = 0.95 * i_offset + 0.05 * (i_raw as f32);
|
||||
q_offset = 0.95 * q_offset + 0.05 * (q_raw as f32);
|
||||
|
||||
*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) - i_offset;
|
||||
let q_sample = (q_raw as f32) - q_offset;
|
||||
|
||||
let i_sample = (i_raw as f32) - *ctx.local.i_offset;
|
||||
let q_sample = (q_raw as f32) - *ctx.local.q_offset;
|
||||
samples[idx] =
|
||||
Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
|
||||
}
|
||||
samples
|
||||
})
|
||||
.unwrap();
|
||||
|
||||
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 mut fft_input = [Complex::<f32>::default(); 128];
|
||||
for idx in 0..samples.len() / 2 {
|
||||
let _filtered = ctx.local.usb_filter.compute(samples[idx]) * 2.0;
|
||||
fft_input[idx] = samples[idx];
|
||||
}
|
||||
|
||||
let spectrum = cfft_128(&mut fft_input);
|
||||
|
||||
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
|
||||
);
|
||||
|
||||
ctx.local.board_led.set_high();
|
||||
|
||||
expected_half = if expected_half == dma::Half::First {
|
||||
|
|
Loading…
Reference in New Issue