From f2382c834a61aeb9d3311677a5f67d018ccfd250 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Oct 2023 12:52:29 +0200 Subject: [PATCH] use Zero,One traits --- src/iir.rs | 23 +++++++++++------------ src/iir_int.rs | 2 +- src/tools.rs | 9 +++++---- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/iir.rs b/src/iir.rs index 1b1d719..3217bc9 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -2,7 +2,7 @@ use serde::{Deserialize, Serialize}; use super::{abs, copysign, macc}; use core::iter::Sum; -use num_traits::{clamp, Float, NumCast}; +use num_traits::{clamp, Float, One, Zero}; /// IIR state and coefficients type. /// @@ -62,11 +62,13 @@ pub struct IIR { pub y_max: T, } -impl> IIR { +impl> IIR { pub fn new(gain: T, y_min: T, y_max: T) -> Self { + let mut ba = [T::zero(); 5]; + ba[0] = gain; Self { - ba: [gain, T::default(), T::default(), T::default(), T::default()], - y_offset: T::default(), + ba, + y_offset: T::zero(), y_min, y_max, } @@ -81,20 +83,17 @@ impl> IIR { /// * `ki` - Integral gain at Nyquist. Sign taken from `kp`. /// * `g` - Gain limit. pub fn set_pi(&mut self, kp: T, ki: T, g: T) -> Result<(), &str> { - let zero: T = T::default(); - let one: T = NumCast::from(1.0).unwrap(); - let two: T = NumCast::from(2.0).unwrap(); let ki = copysign(ki, kp); let g = copysign(g, kp); let (a1, b0, b1) = if abs(ki) < T::epsilon() { - (zero, kp, zero) + (T::zero(), kp, T::zero()) } else { let c = if abs(g) < T::epsilon() { - one + T::one() } else { - one / (one + ki / g) + T::one() / (T::one() + ki / g) }; - let a1 = two * c - one; + let a1 = (T::one() + T::one()) * c - T::one(); let b0 = ki * c + kp; let b1 = ki * c - a1 * kp; if abs(b0 + b1) < T::epsilon() { @@ -102,7 +101,7 @@ impl> IIR { } (a1, b0, b1) }; - self.ba.copy_from_slice(&[b0, b1, zero, a1, zero]); + self.ba.copy_from_slice(&[b0, b1, T::zero(), a1, T::zero()]); Ok(()) } diff --git a/src/iir_int.rs b/src/iir_int.rs index d2ed1b3..0dfd476 100644 --- a/src/iir_int.rs +++ b/src/iir_int.rs @@ -75,7 +75,7 @@ impl IIR { // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; // Compute y0 by multiply-accumulate - let y0 = macc_i32(self.y_offset, xy, &self.ba, IIR::SHIFT); + let y0 = macc_i32(self.y_offset, xy, &self.ba, Self::SHIFT); // Limit y0 let y0 = y0.max(self.y_min).min(self.y_max); // Store y0 x0 x1 y0 y1 y2 diff --git a/src/tools.rs b/src/tools.rs index 5844670..90a9627 100644 --- a/src/tools.rs +++ b/src/tools.rs @@ -1,10 +1,11 @@ use core::ops::{Add, Mul, Neg}; +use num_traits::Zero; pub fn abs(x: T) -> T where - T: PartialOrd + Default + Neg, + T: PartialOrd + Zero + Neg, { - if x >= T::default() { + if x >= T::zero() { x } else { -x @@ -18,9 +19,9 @@ where pub fn copysign(x: T, y: T) -> T where - T: PartialOrd + Default + Neg, + T: PartialOrd + Zero + Neg, { - if (x >= T::default() && y >= T::default()) || (x <= T::default() && y <= T::default()) { + if (x >= T::zero() && y >= T::zero()) || (x <= T::zero() && y <= T::zero()) { x } else { -x