kai
/
pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

lockin: tweak impl

This commit is contained in:
Robert Jördens 2021-01-20 14:07:57 +01:00
parent 778f4ac4d5
commit 507e334ec5
1 changed files with 29 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/main.rs
View File

@ -1010,7 +1010,6 @@ const APP: () = {
c.resources.dacs.1.acquire_buffer(), c.resources.dacs.1.acquire_buffer(),
]; ];
let [dac0, dac1] = dac_samples;
let iir_lockin = c.resources.iir_lockin; let iir_lockin = c.resources.iir_lockin;
let iir_state_lockin = c.resources.iir_state_lockin; let iir_state_lockin = c.resources.iir_state_lockin;
let iir_ch = c.resources.iir_ch; let iir_ch = c.resources.iir_ch;
@ -1024,25 +1023,24 @@ const APP: () = {
let mut phase = let mut phase =
PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC));
dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( for i in 0..adc_samples[0].len() {
|(i, (d0, d1))| {
let m = cossin((phase as i32).wrapping_neg()); let m = cossin((phase as i32).wrapping_neg());
phase = phase.wrapping_add(frequency); phase = phase.wrapping_add(frequency);
let signal = Complex( let signal = Complex(
iir_lockin.update( iir_lockin.update(
&mut iir_state_lockin[0], &mut iir_state_lockin[0],
((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as i32, ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as _,
), ),
iir_lockin.update( iir_lockin.update(
&mut iir_state_lockin[1], &mut iir_state_lockin[1],
((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as i32, ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as _,
), ),
); );
let mut magnitude = let mut magnitude =
(signal.0 * signal.0 + signal.1 * signal.1) as f32; (signal.0 * signal.0 + signal.1 * signal.1) as _;
let mut phase = atan2(signal.1, signal.0) as f32; let mut phase = atan2(signal.1, signal.0) as _;
for j in 0..iir_state[0].len() { for j in 0..iir_state[0].len() {
magnitude = magnitude =
@ -1050,15 +1048,14 @@ const APP: () = {
phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
} }
// Note(unsafe): range clipping to i16 is ensured by IIR filters above.
unsafe { unsafe {
let magnitude = magnitude.to_int_unchecked::<i16>(); dac_samples[0][i] =
let phase = phase.to_int_unchecked::<i16>(); magnitude.to_int_unchecked::<i16>() as u16 ^ 0x8000;
dac_samples[1][i] =
*d0 = magnitude as u16 ^ 0x8000; phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
*d1 = phase as u16 ^ 0x8000; }
} }
},
);
if let Some(dds_output) = c.resources.dds_output { if let Some(dds_output) = c.resources.dds_output {
let builder = dds_output.builder().update_channels( let builder = dds_output.builder().update_channels(