添加滤波算法

CMakeLists.txt

@@ -31,12 +31,17 @@ set(PROJECT_SOURCES
         mainwindow.h
         mainwindow.ui
 
+
         src/qt_serial_datachannel.cpp
         src/electrocardiograph_tester.cpp
+        src/filter_algo_mgr.cpp
 
         libzqt/widgetplot2d.cpp
         libzqt/widgetplot2d.ui
         libzqt/qcustomplot.cpp
+
+        src/algo/iflytop_simple_filter.cpp
+
 )
 
 

CMakeLists.txt.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 5.0.2, 2024-04-01T17:29:11. -->
+<!-- Written by QtCreator 5.0.2, 2024-05-03T15:49:16. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -94,7 +94,7 @@
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.12.12 MinGW 64-bit</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.12.12 MinGW 64-bit</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.qt5.51212.win64_mingw73_kit</value>
-   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">4</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
@@ -347,7 +347,7 @@
     <value type="bool" key="RunConfiguration.UseLibrarySearchPath">true</value>
     <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
     <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
-    <value type="QString" key="RunConfiguration.WorkingDirectory.default">D:/workspace/nordic_wp/electrocardiograph_upper/build</value>
+    <value type="QString" key="RunConfiguration.WorkingDirectory.default">D:/workspace/nordic_wp/build-electrocardiograph_upper-Desktop_Qt_5_12_12_MinGW_64_bit-Release</value>
    </valuemap>
    <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
   </valuemap>

ify_hrs_protocol

@@ -1 +1 @@
-Subproject commit 4ed480c0ba1b2326d4465f8139ee97673f35ffa8
+Subproject commit 391a9551d21fd8b591e1eb4a5c8555ec2d59ff3c

libzqt/logger.cpp

@@ -19,8 +19,12 @@ int32_t zos_get_ticket() { return (int32_t)QDateTime::currentMSecsSinceEpoch();
 std::string zhex2str(const uint8_t* hex, size_t len) {
   std::string str;
   for (size_t i = 0; i < len; i++) {
-    char buf[3] = {0};
-    snprintf(buf, sizeof(buf), "%02x ", hex[i]);
+    char buf[10] = {0};
+    if(i == len - 1) {
+      snprintf(buf, sizeof(buf), "0x%02x", hex[i]);
+    } else {
+      snprintf(buf, sizeof(buf), "0x%02x,", hex[i]);
+    }
     str += buf;
   }
   return str;

mainwindow.cpp

@@ -8,7 +8,9 @@
 
 #include "./ui_mainwindow.h"
 #include "ads129x/ads129x_type.h"
+#include "algo/iflytop_simple_filter.h"
 #include "electrocardiograph_tester.hpp"
+#include "filter_algo_mgr.hpp"
 #include "logger.hpp"
 #include "qt_serial_datachannel.hpp"
 #include "widgetplot2d.h"
@@ -29,6 +31,25 @@ static MainWindow   *m_mainWindow;
 static QTDataChannel G_QTDataChannel;
 static device_type_t m_devicetype;
 
+static const uint32_t str2int(QString str) {
+  // 如果0x开头，按16进制转换
+  // 如果0b开头，按2进制转换
+  // 否则按10进制转换
+
+  // 去除掉str中_
+  str.remove("_");
+
+  if (str.startsWith("0x")) {
+    return str.toUInt(nullptr, 16);
+  } else if (str.startsWith("0b")) {
+    // remove 0b
+    str.remove(0, 2);
+    return str.toUInt(nullptr, 2);
+  } else {
+    return str.toUInt(nullptr, 10);
+  }
+}
+
 #define TAG "MainWindow"
 
 static const char *fmt(const char *fmt, ...) {
@@ -298,11 +319,12 @@ void MainWindow::constructUI() {
             uint32_t timestamp = *(uint32_t *)(&heartrate_report->data[50 * 4]);
 
             reportPreviewShow("[preview data  ] timestamp:%10d lost:%d index %d packetlen %d", timestamp, lostpacket, heartrate_report->sample_data_index, packetlen);
-
             int32_t *frame = (int32_t *)heartrate_report->data;
             for (int i = 0; i < 50; i++) {
               int32_t data       = frame[i];
               int32_t frameIndex = heartrate_report->sample_data_index + i;
+              data = FilterAlgoMgr::ins()->processData("心电", data);
+
               emit doinui_signal(QFunction([this, data, i]() { wp2d->addData("心电", data, i * 2); }));
             }
           }
@@ -570,6 +592,7 @@ void MainWindow::on_TestCmd_readSubIcRegs_clicked() {
     data[ADS129X_REG_RESP1]    = ElectrocardiographTester::ins()->testCmdReadReg(ADS129X_REG_RESP1);
     data[ADS129X_REG_RESP2]    = ElectrocardiographTester::ins()->testCmdReadReg(ADS129X_REG_RESP2);
     data[ADS129X_REG_GPIO]     = ElectrocardiographTester::ins()->testCmdReadReg(ADS129X_REG_GPIO);
+
     ishow("reg %10s %02d: 0x%02x", "ID", ADS129X_REG_ID, data[ADS129X_REG_ID]);
     ishow("reg %10s %02d: 0x%02x", "CONFIG1", ADS129X_REG_CONFIG1, data[ADS129X_REG_CONFIG1]);
     ishow("reg %10s %02d: 0x%02x", "CONFIG2", ADS129X_REG_CONFIG2, data[ADS129X_REG_CONFIG2]);
@@ -583,14 +606,16 @@ void MainWindow::on_TestCmd_readSubIcRegs_clicked() {
     ishow("reg %10s %02d: 0x%02x", "RESP2", ADS129X_REG_RESP2, data[ADS129X_REG_RESP2]);
     ishow("reg %10s %02d: 0x%02x", "GPIO", ADS129X_REG_GPIO, data[ADS129X_REG_GPIO]);
 
+    ishow("regs: %s", zhex2str(data, 12).c_str());
+
   } catch (zexception &exception) {
     processException(exception);
   }
 }
 
 void MainWindow::on_TestCmd_writeSubICReg_clicked() {
-  uint8_t add = ui->TestCmd_writeSubICReg_p0->toPlainText().toInt();
-  uint8_t val = ui->TestCmd_writeSubICReg_p1->toPlainText().toInt();
+  uint32_t add = str2int(ui->TestCmd_writeSubICReg_p0->toPlainText());
+  uint32_t val = str2int(ui->TestCmd_writeSubICReg_p1->toPlainText());
   instructionPreviewClear();
   try {
     ElectrocardiographTester::ins()->testCmdWriteReg(add, val);
@@ -669,3 +694,68 @@ void MainWindow::on_resetDevice_clicked() {
   }
   ishow("call reset device success");
 }
+
+void MainWindow::on_FilterUpdateParameter_clicked() {
+  instructionPreviewClear();
+
+  FilterAlgoMgr::ins()->FilterCommon_setSampleTimeMs(ui->FilterCommon_SampleTimeMs->text().toFloat());
+  FilterAlgoMgr::ins()->LPFilter_setEnable(ui->LPFilter_Enable->isChecked());
+  FilterAlgoMgr::ins()->LPFilter_setCutoffFreqHz(ui->LPFilter_CutoffFreqHz->text().toFloat());
+  FilterAlgoMgr::ins()->LPFilter_setOrder(ui->LPFilter_Order->text().toInt());
+
+  FilterAlgoMgr::ins()->HPFilter_setEnable(ui->HPFilter_Enable->isChecked());
+  FilterAlgoMgr::ins()->HPFilter_setCutoffFreqHz(ui->HPFilter_CutoffFreqHz->text().toFloat());
+  FilterAlgoMgr::ins()->HPFilter_setOrder(ui->HPFilter_Order->text().toInt());
+
+  FilterAlgoMgr::ins()->NOTCHFilter_setEnable(ui->NOTCHFilter_Enable->isChecked());
+  FilterAlgoMgr::ins()->NOTCHFilter_setCenterFreqHz(ui->NOTCHFilter_CenterFreqHz->text().toFloat());
+  FilterAlgoMgr::ins()->NOTCHFilter_setNotchWidthHz(ui->NOTCHFilter_NotchWidthHz->text().toFloat());
+  FilterAlgoMgr::ins()->NOTCHFilter_setOrder(ui->NOTCHFilter_Order->text().toInt());
+
+  FilterAlgoMgr::ins()->updateParameter();
+  on_buttonTabWidget_currentChanged(0);
+
+  ishow("Filter Update Parameter success");
+}
+
+void MainWindow::on_buttonTabWidget_currentChanged(int index) {
+  ui->FilterCommon_SampleTimeMs->setText(QString::number(FilterAlgoMgr::ins()->FilterCommon_getSampleTimeMs()));
+  ui->LPFilter_Enable->setChecked(FilterAlgoMgr::ins()->LPFilter_getEnable());
+  ui->LPFilter_CutoffFreqHz->setText(QString::number(FilterAlgoMgr::ins()->LPFilter_getCutoffFreqHz()));
+  ui->LPFilter_Order->setText(QString::number(FilterAlgoMgr::ins()->LPFilter_getOrder()));
+
+  ui->HPFilter_Enable->setChecked(FilterAlgoMgr::ins()->HPFilter_getEnable());
+  ui->HPFilter_CutoffFreqHz->setText(QString::number(FilterAlgoMgr::ins()->HPFilter_getCutoffFreqHz()));
+  ui->HPFilter_Order->setText(QString::number(FilterAlgoMgr::ins()->HPFilter_getOrder()));
+
+  ui->NOTCHFilter_Enable->setChecked(FilterAlgoMgr::ins()->NOTCHFilter_getEnable());
+  ui->NOTCHFilter_CenterFreqHz->setText(QString::number(FilterAlgoMgr::ins()->NOTCHFilter_getCenterFreqHz()));
+  ui->NOTCHFilter_NotchWidthHz->setText(QString::number(FilterAlgoMgr::ins()->NOTCHFilter_getNotchWidthHz()));
+  ui->NOTCHFilter_Order->setText(QString::number(FilterAlgoMgr::ins()->NOTCHFilter_getOrder()));
+}
+
+void MainWindow::on_TestCmd_writeSubICAllReg_clicked() {
+  // 0x52,0x02,0xe0,0xF1,0x00,0x80,0x00,0x03,0x12,0x02,0x07,0x0c
+  instructionPreviewClear();
+
+  QString     regs    = ui->TestCmd_writeSubICAllReg_p0->toPlainText();
+  QStringList reglist = regs.split(",");
+  if (reglist.size() != 12) {
+    ishow("reg size error %d != 12", reglist.size());
+    return;
+  }
+
+  try {
+    uint8_t data[12];
+    for (int i = 0; i < 12; i++) {
+      data[i] = str2int(reglist[i]);
+    }
+    for (size_t i = 0; i < 12; i++) {
+      ElectrocardiographTester::ins()->testCmdWriteReg(i, data[i]);
+      ishow("write reg %d: 0x%02x success", i, data[i]);
+    }
+    ishow("write all reg success");
+  } catch (zexception &exception) {
+    processException(exception);
+  }
+}

mainwindow.h

@@ -122,6 +122,12 @@ class MainWindow : public QMainWindow {
 
   void on_resetDevice_clicked();
 
+  void on_FilterUpdateParameter_clicked();
+
+  void on_buttonTabWidget_currentChanged(int index);
+
+  void on_TestCmd_writeSubICAllReg_clicked();
+
 signals:
   void doinui_signal(QFunction);
 

src/algo/iflytop_simple_filter.cpp

@@ -0,0 +1,134 @@
+
+#include "iflytop_simple_filter.h"
+
+#include <math.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "logger.hpp"
+
+#define PI 3.141592653
+
+/********************************************************************************************************
+ *                              LOW PASS FILTER
+ ********************************************************************************************************/
+
+void LPFilter_Init(LPFilter_t *filter, float cutoffFreqHz, float sampleTimeS, bool enable) {
+  float RC        = 0.0;
+  RC              = 1.0 / (2 * PI * cutoffFreqHz);
+  filter->coef[0] = sampleTimeS / (sampleTimeS + RC);
+  filter->coef[1] = RC / (sampleTimeS + RC);
+
+  filter->v_out[0] = 0.0;
+  filter->v_out[1] = 0.0;
+
+  filter->enable = enable;
+
+  ZLOGI("FILTER","LPFilter_Init: cutoffFreqHz=%f, sampleTimeS=%f, enable=%d", cutoffFreqHz, sampleTimeS, enable);
+}
+
+float LPFilter_Update(LPFilter_t *filter, float v_in) {
+  if (filter->enable == false) return v_in;
+
+  filter->v_out[1] = filter->v_out[0];
+  filter->v_out[0] = (filter->coef[0] * v_in) + (filter->coef[1] * filter->v_out[1]);
+
+  return (filter->v_out[0]);
+}
+
+/********************************************************************************************************
+ *                              HIGH PASS FILTER
+ ********************************************************************************************************/
+void HPFilter_Init(HPFilter_t *filter, float cutoffFreqHz, float sampleTimeS, bool enable) {
+  float RC = 0.0;
+  RC       = 1.0 / (2 * PI * cutoffFreqHz);
+
+  filter->coef = RC / (sampleTimeS + RC);
+
+  filter->v_in[0] = 0.0;
+  filter->v_in[1] = 0.0;
+
+  filter->v_out[0] = 0.0;
+  filter->v_out[1] = 0.0;
+
+  filter->enable = enable;
+}
+
+float HPFilter_Update(HPFilter_t *filter, float v_in) {
+  if (filter->enable == false) return v_in;
+
+  filter->v_in[1] = filter->v_in[0];
+  filter->v_in[0] = v_in;
+
+  filter->v_out[1] = filter->v_out[0];
+
+  filter->v_out[0] = filter->coef * (filter->v_in[0] - filter->v_in[1] + filter->v_out[1]);
+
+  return (filter->v_out[0]);
+}
+
+/********************************************************************************************************
+ *                              BAND PASS FILTER
+ ********************************************************************************************************/
+
+void PBFilter_Init(PBFilter_t *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS, bool enable) {
+  LPFilter_Init(&filter->lpf, LPF_cutoffFreqHz, sampleTimeS, enable);
+  HPFilter_Init(&filter->hpf, HPF_cutoffFreqHz, sampleTimeS, enable);
+  filter->out_in = 0.0;
+  filter->enable = enable;
+}
+
+float PBFilter_Update(PBFilter_t *filter, float v_in) {
+  if (filter->enable == false) return v_in;
+
+  filter->out_in = HPFilter_Update(&filter->hpf, v_in);
+
+  filter->out_in = LPFilter_Update(&filter->lpf, filter->out_in);
+
+  return (filter->out_in);
+}
+
+/********************************************************************************************************
+ *                              NOTCH FILTER
+ ********************************************************************************************************/
+
+void NOTCHFilter_Init(NOTCHFilter_t *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS, bool enable) {
+  // filter frequency to angular (rad/s)
+  float w0_rps = 2.0 * PI * centerFreqHz;
+  float ww_rps = 2.0 * PI * notchWidthHz;
+
+  // pre warp center frequency
+  float w0_pw_rps = (2.0 / sampleTimeS) * tanf(0.5 * w0_rps * sampleTimeS);
+
+  // computing filter coefficients
+
+  filter->alpha = 4.0 + w0_rps * w0_pw_rps * sampleTimeS * sampleTimeS;
+  filter->beta  = 2.0 * ww_rps * sampleTimeS;
+
+  // clearing input and output  buffers
+
+  for (uint8_t n = 0; n < 3; n++) {
+    filter->vin[n]  = 0;
+    filter->vout[n] = 0;
+  }
+  filter->enable = enable;
+}
+
+float NOTCHFilter_Update(NOTCHFilter_t *filter, float vin) {
+  if (filter->enable == false) return vin;
+
+  // shifting samples
+  filter->vin[2] = filter->vin[1];
+  filter->vin[1] = filter->vin[0];
+
+  filter->vout[2] = filter->vout[1];
+  filter->vout[1] = filter->vout[0];
+
+  filter->vin[0] = vin;
+
+  // compute new output
+  filter->vout[0] = (filter->alpha * filter->vin[0] + 2.0 * (filter->alpha - 8.0) * filter->vin[1] + filter->alpha * filter->vin[2] - (2.0f * (filter->alpha - 8.0) * filter->vout[1] + (filter->alpha - filter->beta) * filter->vout[2])) / (filter->alpha + filter->beta);
+
+  return (filter->vout[0]);
+}

src/algo/iflytop_simple_filter.h

@@ -0,0 +1,72 @@
+#ifndef FILTERS_H
+#define FILTERS_H
+#include <stdbool.h>
+
+// #ifdef __cplusplus
+// extern "C" {
+// #endif
+
+
+/********************************************************************************************************
+ *                              LOW PASS FILTER
+ ********************************************************************************************************/
+typedef struct {
+  float coef[2];
+  float v_out[2];
+  bool  enable;
+} LPFilter_t;
+
+void  LPFilter_Init(LPFilter_t *filter, float cutoffFreqHz, float sampleTimeS, bool enable);
+float LPFilter_Update(LPFilter_t *filter, float v_in);
+
+/********************************************************************************************************
+ *                              HIGH PASS FILTER
+ ********************************************************************************************************/
+typedef struct {
+  float coef;
+  float v_out[2];
+  float v_in[2];
+  bool  enable;
+
+} HPFilter_t;
+
+void  HPFilter_Init(HPFilter_t *filter, float cutoffFreqHz, float sampleTimeS, bool enable);
+float HPFilter_Update(HPFilter_t *filter, float v_in);
+
+/********************************************************************************************************
+ *                              BAND PASS FILTER
+ ********************************************************************************************************/
+
+typedef struct {
+  LPFilter_t lpf;
+  HPFilter_t hpf;
+  float      out_in;
+  bool       enable;
+
+} PBFilter_t;
+
+void  PBFilter_Init(PBFilter_t *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS, bool enable);
+float PBFilter_Update(PBFilter_t *filter, float v_in);
+
+/********************************************************************************************************
+ *                              NOTCH FILTER
+ ********************************************************************************************************/
+
+typedef struct {
+  float alpha;
+  float beta;
+
+  float vin[3];
+  float vout[3];
+  bool  enable;
+
+} NOTCHFilter_t;
+
+void  NOTCHFilter_Init(NOTCHFilter_t *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS, bool enable);
+float NOTCHFilter_Update(NOTCHFilter_t *filter, float vin);
+
+// #ifdef __cplusplus
+// }
+// #endif
+
+#endif

src/filter_algo_mgr.cpp

@@ -0,0 +1,163 @@
+#include "filter_algo_mgr.hpp"
+
+#include <mutex>
+
+#include "ify_hrs_protocol/heart_rate_sensor_protocol.h"
+#include "zexception.hpp"
+
+using namespace iflytop;
+
+void FilterGroup::initialize() {}
+
+int32_t FilterGroup::processData(int32_t data) {  //
+  std::lock_guard<std::recursive_mutex> lock(lock_);
+
+  float v0 = (float)data;
+  for (int i = 0; i < _lpfilter_order; i++) {
+    v0 = LPFilter_Update(&lpfilter[i], v0);
+  }
+
+  for (int i = 0; i < _hpfilter_order; i++) {
+    v0 = HPFilter_Update(&hpfilter[i], v0);
+  }
+
+  for (int i = 0; i < _notchfilter_order; i++) {
+    v0 = NOTCHFilter_Update(&notchfilter[i], v0);
+  }
+
+  return (int32_t)v0;
+}
+
+void FilterGroup::updateParameter() {
+  std::lock_guard<std::recursive_mutex> lock(lock_);
+
+  for (int i = 0; i < FILTER_MAX_ORDER; i++) {
+    LPFilter_Init(&lpfilter[i], LPFilter_cutoffFreqHz, sampleTimeMs / 1000.0, lpfilter_enable);
+  }
+  for (int i = 0; i < FILTER_MAX_ORDER; i++) {
+    HPFilter_Init(&hpfilter[i], HPFilter_cutoffFreqHz, sampleTimeMs / 1000.0, hpfilter_enable);
+  }
+  for (int i = 0; i < FILTER_MAX_ORDER; i++) {
+    NOTCHFilter_Init(&notchfilter[i], NOTCHFilter_centerFreqHz, NOTCHFilter_notchWidthHz, sampleTimeMs / 1000.0, notchfilter_enable);
+  }
+
+  _lpfilter_order    = lpfilter_order;
+  _hpfilter_order    = hpfilter_order;
+  _notchfilter_order = notchfilter_order;
+}
+
+/***********************************************************************************************************************
+ *                                                      通用参数                                                       *
+ ***********************************************************************************************************************/
+
+void FilterGroup::setSampleTimeMs(float sampleTimeMs) {
+  if (sampleTimeMs < 0.4) {
+    throw zexception(kifyhrs_ecode_upper_exception, "sampleTimeMs must be greater than 0.4ms");
+  }
+  this->sampleTimeMs = sampleTimeMs;
+}
+float FilterGroup::getSampleTimeMs() { return sampleTimeMs; }
+
+void FilterGroup::LPFilter_setOrder(int order) {
+  if (order < 1) {
+    order = 1;
+  }
+  if (order > FILTER_MAX_ORDER) {
+    order = FILTER_MAX_ORDER;
+  }
+  lpfilter_order = order;
+}
+int  FilterGroup::LPFilter_getOrder() { return lpfilter_order; }
+void FilterGroup::HPFilter_setOrder(int order) {
+  if (order < 1) {
+    order = 1;
+  }
+  if (order > FILTER_MAX_ORDER) {
+    order = FILTER_MAX_ORDER;
+  }
+  hpfilter_order = order;
+}
+int  FilterGroup::HPFilter_getOrder() { return hpfilter_order; }
+void FilterGroup::NOTCHFilter_setOrder(int order) {
+  if (order < 1) {
+    order = 1;
+  }
+  if (order > FILTER_MAX_ORDER) {
+    order = FILTER_MAX_ORDER;
+  }
+  notchfilter_order = order;
+}
+int FilterGroup::NOTCHFilter_getOrder() { return notchfilter_order; }
+
+/***********************************************************************************************************************
+ *                                                     低通滤波器                                                      *
+ ***********************************************************************************************************************/
+
+void FilterGroup::LPFilter_setEnable(bool enable) { lpfilter_enable = enable; }
+void FilterGroup::LPFilter_setCutoffFreqHz(float cutoffFreqHz) { LPFilter_cutoffFreqHz = cutoffFreqHz; }
+
+bool  FilterGroup::LPFilter_getEnable() { return lpfilter_enable; }
+float FilterGroup::LPFilter_getCutoffFreqHz() { return LPFilter_cutoffFreqHz; }
+
+/***********************************************************************************************************************
+ *                                                     高通滤波器                                                      *
+ ***********************************************************************************************************************/
+void FilterGroup::HPFilter_setEnable(bool enable) { hpfilter_enable = enable; }
+void FilterGroup::HPFilter_setCutoffFreqHz(float cutoffFreqHz) { HPFilter_cutoffFreqHz = cutoffFreqHz; }
+
+bool  FilterGroup::HPFilter_getEnable() { return hpfilter_enable; }
+float FilterGroup::HPFilter_getCutoffFreqHz() { return HPFilter_cutoffFreqHz; }
+
+/***********************************************************************************************************************
+ *                                                     带阻滤波器                                                      *
+ ***********************************************************************************************************************/
+void FilterGroup::NOTCHFilter_setEnable(bool enable) { notchfilter_enable = enable; }
+void FilterGroup::NOTCHFilter_setCenterFreqHz(float centerFreqHz) { NOTCHFilter_centerFreqHz = centerFreqHz; }
+void FilterGroup::NOTCHFilter_setNotchWidthHz(float notchWidthHz) { NOTCHFilter_notchWidthHz = notchWidthHz; }
+
+bool  FilterGroup::NOTCHFilter_getEnable() { return notchfilter_enable; }
+float FilterGroup::NOTCHFilter_getCenterFreqHz() { return NOTCHFilter_centerFreqHz; }
+float FilterGroup::NOTCHFilter_getNotchWidthHz() { return NOTCHFilter_notchWidthHz; }
+
+/***********************************************************************************************************************
+ *                                                    FilterAlgoMgr                                                    *
+ ***********************************************************************************************************************/
+
+FilterAlgoMgr* FilterAlgoMgr::ins() {
+  static FilterAlgoMgr ins;
+  return &ins;
+}
+
+int32_t FilterAlgoMgr::processData(string channel, int32_t data) { return filter.processData(data); }
+
+void FilterAlgoMgr::LPFilter_setEnable(bool enable) { filter.LPFilter_setEnable(enable); }
+void FilterAlgoMgr::LPFilter_setCutoffFreqHz(float cutoffFreqHz) { filter.LPFilter_setCutoffFreqHz(cutoffFreqHz); }
+
+bool  FilterAlgoMgr::LPFilter_getEnable() { return filter.LPFilter_getEnable(); }
+float FilterAlgoMgr::LPFilter_getCutoffFreqHz() { return filter.LPFilter_getCutoffFreqHz(); }
+
+void FilterAlgoMgr::HPFilter_setEnable(bool enable) { filter.HPFilter_setEnable(enable); }
+void FilterAlgoMgr::HPFilter_setCutoffFreqHz(float cutoffFreqHz) { filter.HPFilter_setCutoffFreqHz(cutoffFreqHz); }
+
+bool  FilterAlgoMgr::HPFilter_getEnable() { return filter.HPFilter_getEnable(); }
+float FilterAlgoMgr::HPFilter_getCutoffFreqHz() { return filter.HPFilter_getCutoffFreqHz(); }
+
+void FilterAlgoMgr::NOTCHFilter_setEnable(bool enable) { filter.NOTCHFilter_setEnable(enable); }
+void FilterAlgoMgr::NOTCHFilter_setCenterFreqHz(float centerFreqHz) { filter.NOTCHFilter_setCenterFreqHz(centerFreqHz); }
+void FilterAlgoMgr::NOTCHFilter_setNotchWidthHz(float notchWidthHz) { filter.NOTCHFilter_setNotchWidthHz(notchWidthHz); }
+
+bool  FilterAlgoMgr::NOTCHFilter_getEnable() { return filter.NOTCHFilter_getEnable(); }
+float FilterAlgoMgr::NOTCHFilter_getCenterFreqHz() { return filter.NOTCHFilter_getCenterFreqHz(); }
+float FilterAlgoMgr::NOTCHFilter_getNotchWidthHz() { return filter.NOTCHFilter_getNotchWidthHz(); }
+
+void  FilterAlgoMgr::FilterCommon_setSampleTimeMs(float sampleTimeMs) { filter.setSampleTimeMs(sampleTimeMs); }
+float FilterAlgoMgr::FilterCommon_getSampleTimeMs() { return filter.getSampleTimeMs(); }
+
+void FilterAlgoMgr::LPFilter_setOrder(int order) { filter.LPFilter_setOrder(order); }
+int  FilterAlgoMgr::LPFilter_getOrder() { return filter.LPFilter_getOrder(); }
+void FilterAlgoMgr::HPFilter_setOrder(int order) { filter.HPFilter_setOrder(order); }
+int  FilterAlgoMgr::HPFilter_getOrder() { return filter.HPFilter_getOrder(); }
+void FilterAlgoMgr::NOTCHFilter_setOrder(int order) { filter.NOTCHFilter_setOrder(order); }
+int  FilterAlgoMgr::NOTCHFilter_getOrder() { return filter.NOTCHFilter_getOrder(); }
+
+void FilterAlgoMgr::updateParameter() { filter.updateParameter(); }

src/filter_algo_mgr.hpp

@@ -0,0 +1,156 @@
+#pragma once
+#include <fstream>
+#include <functional>
+#include <iostream>
+#include <list>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <set>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "algo/iflytop_simple_filter.h"
+
+namespace iflytop {
+using namespace std;
+#define FILTER_MAX_ORDER 10
+
+class FilterGroup {
+ private:
+  LPFilter_t    lpfilter[10]    = {0};
+  HPFilter_t    hpfilter[10]    = {0};
+  NOTCHFilter_t notchfilter[10] = {0};
+
+  int _lpfilter_order    = 1;
+  int _hpfilter_order    = 1;
+  int _notchfilter_order = 1;
+
+  /***********************************************************************************************************************
+   *                                                      缓存参数                                                       *
+   ***********************************************************************************************************************/
+
+  bool lpfilter_enable    = false;
+  bool hpfilter_enable    = false;
+  bool notchfilter_enable = false;
+
+  int lpfilter_order    = 1;
+  int hpfilter_order    = 1;
+  int notchfilter_order = 1;
+
+  float LPFilter_cutoffFreqHz    = 0;
+  float HPFilter_cutoffFreqHz    = 0;
+  float NOTCHFilter_centerFreqHz = 0;
+  float NOTCHFilter_notchWidthHz = 0;
+
+  float sampleTimeMs = 2;  // 1ms
+
+  std::recursive_mutex lock_;
+
+ public:
+  void initialize();
+
+  int32_t processData(int32_t data);
+
+  void updateParameter();
+
+  /***********************************************************************************************************************
+   *                                                      通用参数                                                       *
+   ***********************************************************************************************************************/
+  void  setSampleTimeMs(float sampleTimeMs);
+  float getSampleTimeMs();
+
+  /***********************************************************************************************************************
+   *                                                     低通滤波器                                                      *
+   ***********************************************************************************************************************/
+
+  void LPFilter_setEnable(bool enable);
+  void LPFilter_setCutoffFreqHz(float cutoffFreqHz);
+  void LPFilter_setOrder(int order);
+
+  bool  LPFilter_getEnable();
+  float LPFilter_getCutoffFreqHz();
+  int   LPFilter_getOrder();
+
+  /***********************************************************************************************************************
+   *                                                     高通滤波器                                                      *
+   ***********************************************************************************************************************/
+  void HPFilter_setEnable(bool enable);
+  void HPFilter_setCutoffFreqHz(float cutoffFreqHz);
+  void HPFilter_setOrder(int order);
+
+  bool  HPFilter_getEnable();
+  float HPFilter_getCutoffFreqHz();
+  int   HPFilter_getOrder();
+
+  /***********************************************************************************************************************
+   *                                                     带阻滤波器                                                      *
+   ***********************************************************************************************************************/
+  void NOTCHFilter_setEnable(bool enable);
+  void NOTCHFilter_setCenterFreqHz(float centerFreqHz);
+  void NOTCHFilter_setNotchWidthHz(float notchWidthHz);
+  void NOTCHFilter_setOrder(int order);
+
+  bool  NOTCHFilter_getEnable();
+  float NOTCHFilter_getCenterFreqHz();
+  float NOTCHFilter_getNotchWidthHz();
+  int   NOTCHFilter_getOrder();
+};
+
+class FilterAlgoMgr {
+ private:
+  FilterGroup filter;
+
+ public:
+  FilterAlgoMgr(/* args */) {}
+  ~FilterAlgoMgr() {}
+
+  static FilterAlgoMgr* ins();
+
+  int32_t processData(string channel, int32_t data);
+
+  void updateParameter();
+
+  void  FilterCommon_setSampleTimeMs(float sampleTimeMs);
+  float FilterCommon_getSampleTimeMs();
+
+  /***********************************************************************************************************************
+   *                                                     低通滤波器                                                      *
+   ***********************************************************************************************************************/
+
+  void LPFilter_setEnable(bool enable);
+  void LPFilter_setCutoffFreqHz(float cutoffFreqHz);
+  void LPFilter_setOrder(int order);
+
+  bool  LPFilter_getEnable();
+  float LPFilter_getCutoffFreqHz();
+  int   LPFilter_getOrder();
+
+  /***********************************************************************************************************************
+   *                                                     高通滤波器                                                      *
+   ***********************************************************************************************************************/
+  void HPFilter_setEnable(bool enable);
+  void HPFilter_setCutoffFreqHz(float cutoffFreqHz);
+  void HPFilter_setOrder(int order);
+
+  bool  HPFilter_getEnable();
+  float HPFilter_getCutoffFreqHz();
+  int   HPFilter_getOrder();
+
+  /***********************************************************************************************************************
+   *                                                     带阻滤波器                                                      *
+   ***********************************************************************************************************************/
+  void NOTCHFilter_setEnable(bool enable);
+  void NOTCHFilter_setCenterFreqHz(float centerFreqHz);
+  void NOTCHFilter_setNotchWidthHz(float notchWidthHz);
+  void NOTCHFilter_setOrder(int order);
+
+  bool  NOTCHFilter_getEnable();
+  float NOTCHFilter_getCenterFreqHz();
+  float NOTCHFilter_getNotchWidthHz();
+  int   NOTCHFilter_getOrder();
+};
+
+}  // namespace iflytop