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

Merge pull request #252 from quartiq/rj/misc-cleanup 

Rj/misc cleanup
This commit is contained in:
Robert Jördens 2021-02-01 19:10:59 +01:00 committed by GitHub
parent 7e82fce4f0 f02d3cc95b
commit 08cc2e4840
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 88 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
dsp/src/accu.rs
View File

@ -12,7 +12,6 @@ impl Accu {
impl Iterator for Accu {
type Item = i32;
#[inline]
fn next(&mut self) -> Option<i32> {
let s = self.state;
self.state = self.state.wrapping_add(self.step);

1
dsp/src/complex.rs
View File

@ -14,7 +14,6 @@ impl Complex<i32> {
/// Complex::<i32>::from_angle(1 << 30); // pi/2
/// Complex::<i32>::from_angle(-1 << 30); // -pi/2
/// ```
#[inline(always)]
pub fn from_angle(angle: i32) -> Self {
let (c, s) = cossin(angle);
Self(c, s)

9
dsp/src/iir.rs
View File

@ -48,6 +48,15 @@ pub struct IIR {
}
impl IIR {
pub const fn new(gain: f32, y_min: f32, y_max: f32) -> Self {
Self {
ba: Vec5([gain, 0., 0., 0., 0.]),
y_offset: 0.,
y_min,
y_max,
}
}
/// Configures IIR filter coefficients for proportional-integral behavior
/// with gain limit.
///

32
dsp/src/lib.rs
View File

@ -3,38 +3,6 @@
use core::ops::{Add, Mul, Neg};
/// Bit shift, round up half.
///
/// # Arguments
///
/// `x` - Value to shift and round.
/// `shift` - Number of bits to right shift `x`.
///
/// # Returns
///
/// Shifted and rounded value.
#[inline(always)]
pub fn shift_round(x: i32, shift: usize) -> i32 {
(x + (1 << (shift - 1))) >> shift
}
/// Integer division, round up half.
///
/// # Arguments
///
/// `dividend` - Value to divide.
/// `divisor` - Value that divides the
/// dividend. `dividend`+`divisor`-1 must be inside [i64::MIN,
/// i64::MAX].
///
/// # Returns
///
/// Divided and rounded value.
#[inline(always)]
pub fn divide_round(dividend: i64, divisor: i64) -> i64 {
(dividend + (divisor - 1)) / divisor
}
fn abs<T>(x: T) -> T
where
T: PartialOrd + Default + Neg<Output = T>,

5
dsp/src/lockin.rs
View File

@ -14,10 +14,7 @@ impl Lockin {
/// Create a new Lockin with given IIR coefficients.
pub fn new(ba: Vec5) -> Self {
Self {
iir: IIR {
ba,
..Default::default()
},
iir: IIR { ba },
state: [Vec5::default(); 2],
}
}

14
dsp/src/rpll.rs
View File

@ -49,7 +49,7 @@ impl RPLL {
shift_frequency: u8,
shift_phase: u8,
) -> (i32, u32) {
debug_assert!(shift_frequency > self.dt2);
debug_assert!(shift_frequency >= self.dt2);
debug_assert!(shift_phase >= self.dt2);
// Advance phase
self.y = self.y.wrapping_add(self.f as i32);
@ -66,7 +66,7 @@ impl RPLL {
// Reference phase (1 << dt2 full turns) with gain/attenuation applied
let p_ref = 1u32 << (32 + self.dt2 - shift_frequency);
// Update frequency lock
self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
// Time in counter cycles between timestamp and "now"
let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32;
// Reference phase estimate "now"
@ -122,6 +122,10 @@ mod test {
}
fn run(&mut self, n: usize) -> (Vec<f32>, Vec<f32>) {
assert!(self.period >= 1 << self.dt2);
assert!(self.period < 1 << self.shift_frequency);
assert!(self.period < 1 << self.shift_phase + 1);
let mut y = Vec::<f32>::new();
let mut f = Vec::<f32>::new();
for _ in 0..n {
@ -162,10 +166,6 @@ mod test {
}
fn measure(&mut self, n: usize, limits: [f32; 4]) {
assert!(self.period >= 1 << self.dt2);
assert!(self.dt2 <= self.shift_frequency);
assert!(self.period < 1 << self.shift_frequency);
assert!(self.period < 1 << self.shift_frequency + 1);
let t_settle = (1 << self.shift_frequency - self.dt2 + 4)
+ (1 << self.shift_phase - self.dt2 + 4);
self.run(t_settle);
@ -190,7 +190,7 @@ mod test {
print!("{:.2e} ", rel);
assert!(
rel <= 1.,
"idx {}, have |{}| > want {}",
"idx {}, have |{:.2e}| > limit {:.2e}",
i,
m[i],
limits[i]

4
src/bin/dual-iir.rs
View File

@ -17,7 +17,7 @@ use stabilizer::{hardware, server};
use dsp::iir;
use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
const SCALE: f32 = ((1 << 15) - 1) as f32;
const SCALE: f32 = i16::MAX as _;
const TCP_RX_BUFFER_SIZE: usize = 8192;
const TCP_TX_BUFFER_SIZE: usize = 8192;
@ -36,7 +36,7 @@ const APP: () = {
// Format: iir_state[ch][cascade-no][coeff]
#[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
#[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
#[init([[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
}

64
src/bin/lockin-external.rs
View File

@ -21,7 +21,7 @@ use hardware::{
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
};
const SCALE: f32 = ((1 << 15) - 1) as f32;
const SCALE: f32 = i16::MAX as _;
const TCP_RX_BUFFER_SIZE: usize = 8192;
const TCP_TX_BUFFER_SIZE: usize = 8192;
@ -40,7 +40,7 @@ const APP: () = {
// Format: iir_state[ch][cascade-no][coeff]
#[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
#[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
#[init([[iir::IIR::new(1./(1 << 16) as f32, -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
timestamper: InputStamper,
@ -119,21 +119,23 @@ const APP: () = {
let (pll_phase, pll_frequency) = c.resources.pll.update(
c.resources.timestamper.latest_timestamp().map(|t| t as i32),
23, // relative PLL frequency bandwidth: 2**-23, TODO: expose
22, // relative PLL phase bandwidth: 2**-22, TODO: expose
22, // frequency settling time (log2 counter cycles), TODO: expose
22, // phase settling time, TODO: expose
);
// Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate)
let harmonic: i32 = -1;
// Demodulation LO phase offset
let phase_offset: i32 = 0;
let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
as i32) // TODO: maybe rounding bias
let harmonic: i32 = -1; // TODO: expose
// Demodulation LO phase offset
let phase_offset: i32 = 0; // TODO: expose
let sample_frequency = ((pll_frequency
// .wrapping_add(1 << SAMPLE_BUFFER_SIZE_LOG2 - 1) // half-up rounding bias
>> SAMPLE_BUFFER_SIZE_LOG2) as i32)
.wrapping_mul(harmonic);
let sample_phase =
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
if let Some(output) = adc_samples[0]
let output = adc_samples[0]
.iter()
.zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample.
@ -141,27 +143,35 @@ const APP: () = {
lockin.update((sample as i16 as i32) << 16, phase)
})
.last()
{
.unwrap();
// convert i/q to power/phase,
let power_phase = true; // TODO: expose
let mut output = if power_phase {
// Convert from IQ to power and phase.
let mut power = output.abs_sqr() as _;
let mut phase = output.arg() as _;
[output.abs_sqr() as _, output.arg() as _]
} else {
[output.0 as _, output.1 as _]
};
// Filter power and phase through IIR filters.
// Note: Normalization to be done in filters. Phase will wrap happily.
for j in 0..iir_state[0].len() {
power = iir_ch[0][j].update(&mut iir_state[0][j], power);
phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
// Filter power and phase through IIR filters.
// Note: Normalization to be done in filters. Phase will wrap happily.
for j in 0..iir_state[0].len() {
for k in 0..output.len() {
output[k] =
iir_ch[k][j].update(&mut iir_state[k][j], output[k]);
}
}
// Note(unsafe): range clipping to i16 is ensured by IIR filters above.
// Convert to DAC data.
for i in 0..dac_samples[0].len() {
unsafe {
dac_samples[0][i] =
(power.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
dac_samples[1][i] =
(phase.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
}
// Note(unsafe): range clipping to i16 is ensured by IIR filters above.
// Convert to DAC data.
for i in 0..dac_samples[0].len() {
unsafe {
dac_samples[0][i] =
output[0].to_int_unchecked::<i16>() as u16 ^ 0x8000;
dac_samples[1][i] =
output[1].to_int_unchecked::<i16>() as u16 ^ 0x8000;
}
}
}

58
src/bin/lockin-internal.rs
View File

@ -3,13 +3,16 @@
#![no_main]
#![cfg_attr(feature = "nightly", feature(core_intrinsics))]
// A constant sinusoid to send on the DAC output.
const DAC_SEQUENCE: [f32; 8] =
[0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
use dsp::{iir_int, lockin::Lockin, Accu};
use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
use stabilizer::hardware;
use stabilizer::{hardware, SAMPLE_BUFFER_SIZE, SAMPLE_BUFFER_SIZE_LOG2};
// A constant sinusoid to send on the DAC output.
// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
const ONE: i16 = (0.1 * u16::MAX as f32) as _;
const SQRT2: i16 = (ONE as f32 * 0.707) as _;
const DAC_SEQUENCE: [i16; SAMPLE_BUFFER_SIZE] =
[0, SQRT2, ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2]; // TODO: rotate by -2 samples to start with ONE
#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
const APP: () = {
@ -74,45 +77,48 @@ const APP: () = {
];
// DAC0 always generates a fixed sinusoidal output.
for (i, value) in DAC_SEQUENCE.iter().enumerate() {
// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
let y = value * (0.1 * i16::MAX as f32);
// Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
let y = unsafe { y.to_int_unchecked::<i16>() };
// Convert to DAC code
dac_samples[0][i] = y as u16 ^ 0x8000;
}
dac_samples[0]
.iter_mut()
.zip(DAC_SEQUENCE.iter())
.for_each(|(d, s)| *d = *s as u16 ^ 0x8000);
// Reference phase and frequency are known.
let pll_phase = 0;
// 1/8 of the sample rate: log2(DAC_SEQUENCE.len()) == 3
let pll_frequency = 1i32 << (32 - 3);
let pll_frequency = 1i32 << (32 - SAMPLE_BUFFER_SIZE_LOG2);
// Harmonic index of the LO: -1 to _de_modulate the fundamental
let harmonic: i32 = -1;
// Demodulation LO phase offset
let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32;
let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32; // TODO: adapt to sequence rotation above
let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
let sample_phase = phase_offset
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
if let Some(output) = adc_samples
let output = adc_samples
.iter()
// Zip in the LO phase.
.zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample.
// Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| {
lockin.update((sample as i16 as i32) << 16, phase)
})
// Decimate
.last()
{
// Convert from IQ to power and phase.
let _power = output.abs_sqr();
let phase = output.arg() >> 16;
.unwrap();
for value in dac_samples[1].iter_mut() {
*value = phase as u16 ^ 0x8000;
}
// convert i/q to power/phase,
let power_phase = true; // TODO: expose
let output = if power_phase {
// Convert from IQ to power and phase.
[output.abs_sqr(), output.arg()]
} else {
[output.0, output.1]
};
for value in dac_samples[1].iter_mut() {
*value = (output[1] >> 16) as u16 ^ 0x8000;
}
}