bpf_2.tpp

Go to the documentation of this file.

#include "../../include/band_pass/bpf_2.h"
 
template <typename T>
filter::band_pass::BPF_2<T>::BPF_2()
{
   reset();
}
 
template <typename T>
void filter::band_pass::BPF_2<T>::config(T f0_, T B_, T dt_)
{
   set_param(f0_, B_, dt_);
}
 
template <typename T>
void filter::band_pass::BPF_2<T>::set_param(T f0_, T B_, T dt_)
{
   f0 = f0_;
   B = B_;
   dt = dt_;
   Q = f0 / B;
   T wc = 6.28318530717958647692528676655900576 * f0;
 
   T w1 = 0.5 * (std::sqrt(B * B + 4.0 * wc * wc) - B);
   if (w1 < 0)
   {
      w1 = 0;
   }
 
   T w2 = 0.5 * (std::sqrt(B * B + 4.0 * wc * wc) + B);
 
   T w1_n = prewarp_freq(w1, dt);
   T w2_n = prewarp_freq(w2, dt);
 
   T wc_n = std::sqrt(w1_n * w2_n);
   T B_n = w2_n - w1_n;
 
   T Q_n = (wc_n / 6.28318530717958647692528676655900576) / B_n;
 
   T b0 = 2.0 * dt;
   T b1 = 0.0;
   T b2 = -b0;
   T a0 = dt * dt * wc_n * wc_n + (2.0 * dt * wc_n) / Q_n + 4.0;
   T a1 = 2.0 * dt * dt * wc_n * wc_n - 8.0;
   T a2 = dt * dt * wc_n * wc_n - (2.0 * dt * wc_n) / Q_n + 4.0;
 
   lambda_1 = b0 / a0;
   lambda_2 = b1 / a0;
   lambda_3 = b2 / a0;
   lambda_4 = -a1 / a0;
   lambda_5 = -a2 / a0;
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::update(T x_k)
{
   T y_k = 0.0;
 
   if (start_counter <= 1)
   {
      y_k = x_k;
      start_counter++;
   }
   else
   {
      y_k = lambda_1 * x_k + lambda_2 * x_k_1 + lambda_3 * x_k_2 + lambda_4 * y_k_1 + lambda_5 * y_k_2;
   }
   y_k_2 = y_k_1;
   y_k_1 = y_k;
   x_k_2 = x_k_1;
   x_k_1 = x_k;
 
   return y_k;
}
 
template <typename T>
void filter::band_pass::BPF_2<T>::reset()
{
   start_counter = 0;
   y_k_1 = 0.0;
   y_k_2 = 0.0;
   x_k_1 = 0.0;
   x_k_2 = 0.0;
}
 
template <typename T>
void filter::band_pass::BPF_2<T>::set_f0(T f0_)
{
   set_param(f0_, B, dt);
}
 
template <typename T>
void filter::band_pass::BPF_2<T>::set_B(T B_)
{
   set_param(f0, B_, dt);
}
 
template <typename T>
void filter::band_pass::BPF_2<T>::set_dt(T dt_)
{
   set_param(f0, B, dt_);
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::get_f0()
{
   return f0;
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::get_B()
{
   return B;
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::get_dt()
{
   return dt;
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::get_Q()
{
   return Q;
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::get_fs()
{
   return 1.0 / dt;
}
 
template <typename T>
T filter::band_pass::BPF_2<T>::prewarp_freq(T wc_, T dt_)
{
   return (2.0 * std::tan(0.5 * wc_ * dt_)) / dt_;
}