Diff: Biquad.hpp

Revision:

0:fd55988037cc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Biquad.hpp	Tue Sep 13 07:48:33 2022 +0000
@@ -0,0 +1,40 @@
+//--------------------------------------------------------------
+// 縦続形 IIR フィルタの構成要素として使う 2 次の IIR フィルタ
+//      b0 は 1 と仮定している
+//
+// 2022/01/18, Copyright (c) 2022 MIKAMI, Naoki
+//--------------------------------------------------------------
+
+#include "mbed.h"
+
+#ifndef IIR_BIQUAD_HPP
+#define IIR_BIQUAD_HPP
+
+class Biquad
+{
+public:
+    // フィルタの係数に対応する構造体
+    struct Coefs { float a1, a2, b1, b2; };
+
+    // コンストラクタ
+    Biquad(const Coefs ck = (Coefs){0, 0, 0, 0})
+        : a1_(ck.a1), a2_(ck.a2), b1_(ck.b1), b2_(ck.b2),
+          vn1_(0), vn2_(0) {}       
+
+    // 2 次のフィルタを実行する
+    float Execute(float xn)
+    {
+        float vn = xn + a1_*vn1_ + a2_*vn2_;
+        float yn = vn + b1_*vn1_ + b2_*vn2_;
+    
+        vn2_ = vn1_;
+        vn1_ = vn;
+    
+        return yn;
+    }
+
+private:
+    float a1_, a2_, b1_, b2_;   // フィルタの係数
+    float vn1_, vn2_;           // 遅延器
+};
+#endif  // IIR_BIQUAD_HPP
\ No newline at end of file