complex: log2, update bins

This commit is contained in:
Robert Jördens 2021-02-11 18:14:28 +01:00
parent 3ae0b710bc
commit b49f0a2eb9
3 changed files with 38 additions and 14 deletions

View File

@ -21,19 +21,43 @@ impl Complex<i32> {
/// Return the absolute square (the squared magnitude). /// Return the absolute square (the squared magnitude).
/// ///
/// Note: Normalization is `1 << 31`, i.e. Q0.31. /// Note: Normalization is `1 << 32`, i.e. U0.32.
///
/// Note(panic): This will panic for `Complex(i32::MIN, i32::MIN)`
/// ///
/// Example: /// Example:
/// ///
/// ``` /// ```
/// use dsp::Complex; /// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1); /// assert_eq!(Complex(i32::MIN, 0).abs_sqr(), 1 << 31);
/// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1); /// assert_eq!(Complex(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3);
/// ``` /// ```
pub fn abs_sqr(&self) -> i32 { pub fn abs_sqr(&self) -> u32 {
(((self.0 as i64) * (self.0 as i64) (((self.0 as i64) * (self.0 as i64)
+ (self.1 as i64) * (self.1 as i64)) + (self.1 as i64) * (self.1 as i64))
>> 31) as i32 >> 31) as u32
}
/// log2(power) re full scale approximation
///
/// TODO: scale up, interpolate
///
/// Panic:
/// This will panic for `Complex(i32::MIN, i32::MIN)`
///
/// Example:
///
/// ```
/// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, i32::MAX).log2(), -1);
/// assert_eq!(Complex(i32::MAX, 0).log2(), -2);
/// assert_eq!(Complex(1, 0).log2(), -63);
/// assert_eq!(Complex(0, 0).log2(), -64);
/// ```
pub fn log2(&self) -> i32 {
let a = (self.0 as i64) * (self.0 as i64)
+ (self.1 as i64) * (self.1 as i64);
-(a.leading_zeros() as i32)
} }
/// Return the angle. /// Return the angle.
@ -44,12 +68,12 @@ impl Complex<i32> {
/// ///
/// ``` /// ```
/// use dsp::Complex; /// use dsp::Complex;
/// assert_eq!(Complex(i32::MAX, 0).arg(), 0); /// assert_eq!(Complex(1, 0).arg(), 0);
/// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX); /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX);
/// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX); /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX);
/// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1); /// assert_eq!(Complex(0, -1).arg(), -i32::MAX >> 1);
/// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1); /// assert_eq!(Complex(0, 1).arg(), (i32::MAX >> 1) + 1);
/// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1); /// assert_eq!(Complex(1, 1).arg(), (i32::MAX >> 2) + 1);
/// ``` /// ```
pub fn arg(&self) -> i32 { pub fn arg(&self) -> i32 {
atan2(self.1, self.0) atan2(self.1, self.0)

View File

@ -14,16 +14,16 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
/// Update the filter with a new sample. /// Update the filter with a new sample.
/// ///
/// # Args /// # Args
/// * `x`: Input data, needs k bits headroom /// * `x`: Input data
/// * `k`: Log2 time constant, 0..31 /// * `k`: Log2 time constant, 0..31
/// ///
/// # Return /// # Return
/// Filtered output y /// Filtered output y, needs `k` bits headroom
pub fn update(&mut self, x: i32, k: u8) -> i32 { pub fn update(&mut self, x: i32, k: u8) -> i32 {
debug_assert!(k & 31 == k); debug_assert!(k & 31 == k);
// This is an unrolled and optimized first-order IIR loop // This is an unrolled and optimized first-order IIR loop
// that works for all possible time constants. // that works for all possible time constants.
// Note DF-II and the zero(s) at Nyquist. // Note DF-II and the zeros at Nyquist.
let mut x = x; let mut x = x;
for y in self.y.iter_mut() { for y in self.y.iter_mut() {
let dy = x - (*y >> k); let dy = x - (*y >> k);

View File

@ -126,8 +126,8 @@ const APP: () = {
let conf = "frequency_discriminator"; let conf = "frequency_discriminator";
let output = match conf { let output = match conf {
// Convert from IQ to power and phase. // Convert from IQ to power and phase.
"power_phase" => [output.abs_sqr(), output.arg()], "power_phase" => [(output.log2() << 10) as _, output.arg()],
"frequency_discriminator" => [pll_frequency as i32, output.arg()], "frequency_discriminator" => [pll_frequency as _, output.arg()],
_ => [output.0 << 16, output.1 << 16], _ => [output.0 << 16, output.1 << 16],
}; };