From cb72c2ba74a5a10b170c12fcafa930bf094aa1c2 Mon Sep 17 00:00:00 2001
From: "J.A. de Jong - Redu-Sone B.V., ASCEE V.O.F" <j.a.dejong@ascee.nl>
Date: Thu, 20 Oct 2022 16:31:55 +0200
Subject: [PATCH] Added first order high pass static method

---
 src/lasp/dsp/lasp_biquadbank.cpp | 39 ++++++++++++++++++++++++++++++++
 src/lasp/dsp/lasp_biquadbank.h   | 11 +++++++++
 2 files changed, 50 insertions(+)

diff --git a/src/lasp/dsp/lasp_biquadbank.cpp b/src/lasp/dsp/lasp_biquadbank.cpp
index 115f6e3..09ec834 100644
--- a/src/lasp/dsp/lasp_biquadbank.cpp
+++ b/src/lasp/dsp/lasp_biquadbank.cpp
@@ -41,6 +41,45 @@ SeriesBiquad::SeriesBiquad(const vd &filter_coefs) {
         "Filter coefficients should have fourth element (a0) equal to 1.0");
   }
 }
+
+SeriesBiquad SeriesBiquad::firstOrderHighPass(const d fs, const d cuton_Hz) {
+
+  if(fs <= 0) {
+    throw rte("Invalid sampling frequency: " + std::to_string(fs) + " [Hz]");
+  }
+  if(cuton_Hz <= 0) {
+    throw rte("Invalid cuton frequency: " + std::to_string(cuton_Hz) + " [Hz]");
+  }
+  if(cuton_Hz >= 0.98*fs/2) {
+    throw rte("Invalid cuton frequency. We limit this to 0.98* fs / 2. Given value" + std::to_string(cuton_Hz) + " [Hz]");
+  }
+
+  const d tau = 1/(2*arma::datum::pi*cuton_Hz);
+  const d facnum = 2*fs*tau/(1+2*fs*tau);
+  const d facden = (1-2*fs*tau)/(1+2*fs*tau);
+
+  vd coefs(6);
+  // b0
+  coefs(0) = facnum;
+  // b1
+  coefs(1) = -facnum;
+  // b2
+  coefs(2) = 0;
+
+  // a0
+  coefs(3) = 1;
+
+  // a1
+  coefs(4) = facden;
+
+  // a2
+  coefs(5) = 0;
+
+  return SeriesBiquad(coefs);
+
+}
+
+
 std::unique_ptr<Filter> SeriesBiquad::clone() const {
   // sos.as_col() concatenates all columns, exactly what we want.
   return std::make_unique<SeriesBiquad>(sos.as_col());
diff --git a/src/lasp/dsp/lasp_biquadbank.h b/src/lasp/dsp/lasp_biquadbank.h
index c37df12..1426cb5 100644
--- a/src/lasp/dsp/lasp_biquadbank.h
+++ b/src/lasp/dsp/lasp_biquadbank.h
@@ -39,6 +39,17 @@ public:
   virtual ~SeriesBiquad() override {}
   void reset() override final;
   std::unique_ptr<Filter> clone() const override final;
+
+  /**
+   * @brief Create a SeriesBiquad object for a first order high-pass filter
+   *
+   * @param fs Sampling frequency [Hz]
+   * @param cuton_Hz Cuton-frequency [Hz]
+   *
+   * @return SeriesBiquad object with a single biquad, corresponding to a first
+   * order high pass filter.
+   */
+  static SeriesBiquad firstOrderHighPass(const d fs, const d cuton_Hz);
 };
 
 /**