From 7f276bcb1a87bea4b23ec0243f73af2cde5f7247 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Fri, 3 May 2024 19:25:20 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=BB=A4=E6=B3=A2=E7=AE=97?= =?UTF-8?q?=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 5 + CMakeLists.txt.user | 6 +- ify_hrs_protocol | 2 +- libzqt/logger.cpp | 8 +- mainwindow.cpp | 98 +- mainwindow.h | 6 + mainwindow.ui | 2027 ++++++++++++++++++++++++------------ src/algo/iflytop_simple_filter.cpp | 134 +++ src/algo/iflytop_simple_filter.h | 72 ++ src/filter_algo_mgr.cpp | 163 +++ src/filter_algo_mgr.hpp | 156 +++ 11 files changed, 1988 insertions(+), 689 deletions(-) create mode 100644 src/algo/iflytop_simple_filter.cpp create mode 100644 src/algo/iflytop_simple_filter.h create mode 100644 src/filter_algo_mgr.cpp create mode 100644 src/filter_algo_mgr.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 75bcd6d..1cafae2 100644 --- a/CMakeLists.txt +++ b/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 + ) diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user index 0a37cc2..cb97507 100644 --- a/CMakeLists.txt.user +++ b/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -94,7 +94,7 @@ Desktop Qt 5.12.12 MinGW 64-bit Desktop Qt 5.12.12 MinGW 64-bit qt.qt5.51212.win64_mingw73_kit - 4 + 0 0 0 @@ -347,7 +347,7 @@ true false true - D:/workspace/nordic_wp/electrocardiograph_upper/build + D:/workspace/nordic_wp/build-electrocardiograph_upper-Desktop_Qt_5_12_12_MinGW_64_bit-Release 1 diff --git a/ify_hrs_protocol b/ify_hrs_protocol index 4ed480c..391a955 160000 --- a/ify_hrs_protocol +++ b/ify_hrs_protocol @@ -1 +1 @@ -Subproject commit 4ed480c0ba1b2326d4465f8139ee97673f35ffa8 +Subproject commit 391a9551d21fd8b591e1eb4a5c8555ec2d59ff3c diff --git a/libzqt/logger.cpp b/libzqt/logger.cpp index ce8558f..bbc13f2 100644 --- a/libzqt/logger.cpp +++ b/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; diff --git a/mainwindow.cpp b/mainwindow.cpp index 03ec749..4189c9f 100644 --- a/mainwindow.cpp +++ b/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,12 +319,13 @@ 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; - emit doinui_signal(QFunction([this, data, i]() { wp2d->addData("心电", data, i * 2); })); + 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); + } +} diff --git a/mainwindow.h b/mainwindow.h index bfa0748..921b5e7 100644 --- a/mainwindow.h +++ b/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); diff --git a/mainwindow.ui b/mainwindow.ui index 2e00914..69d6e91 100644 --- a/mainwindow.ui +++ b/mainwindow.ui @@ -7,7 +7,7 @@ 0 0 1532 - 1044 + 1076 @@ -17,7 +17,7 @@ - 心电单(三)导上位机 + IFLYTOP-ECG-上位机 /* @@ -578,7 +578,23 @@ QScrollBar::add-page:horizontal, QScrollBar::sub-page:horizontal { } QScrollBar::add-page:vertical, QScrollBar::sub-page:vertical { background: none; -} +} +QGroupBox { + border-color: rgb(156, 156, 156); + border-width: 1px; + border-style: solid; + border-radius:8px; + padding: 10px 0px 0px 0px; + +} + +QGroupBox:title { + subcontrol-origin: margin; + left: 7px; + padding: 0px 0px 0px 0px; +} + + @@ -824,6 +840,25 @@ p, li { white-space: pre-wrap; } GroupBox + + + + 110 + 50 + 147 + 1 + + + + + 16777215 + 1 + + + + + + @@ -852,7 +887,7 @@ p, li { white-space: pre-wrap; } - + 0 @@ -871,682 +906,1316 @@ p, li { white-space: pre-wrap; } 16777215 - - ButtonBox + + 1 - - - - - - 0 - 200 - - - - 采集&预览 - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 停止预览 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 停止采集 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 开始采集 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 开始预览 - - - - - - - - - - 设备信息 - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 读取设备状态信息 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 同步时间 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 读取设备时间 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 读取设备SN - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 读取设备版本信息 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 读取传感器信息 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 复位设备 - - - - - - - - - - 记录上传 - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 读取所有记录 - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 上传记录 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 停止上传 - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 删除记录 - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - - - - 0 - 300 - - - - 测试指令 - - - - - - - 0 - 30 - - - - 停止采集 - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - 0 - 30 - - - - 写ECG寄存器 - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - 0 - 30 - - - - 开始采集 - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - 1 - 0 - - - - - 0 - 30 - - - - 写ECG寄存器() - - - - - - - - 0 - 30 - - - - 读取ECG芯片寄存器 - - - - - - - - 1 - 0 - - - - - 0 - 0 - - - - - 16777215 - 30 - - - - - - - - - 0 - 30 - - - - 修改信号源为方波 - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - + + + 设备基本操作 + + + + + + 设备信息 + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 读取设备状态信息 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 同步时间 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 读取设备时间 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 读取设备SN + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 读取设备版本信息 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 读取传感器信息 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 复位设备 + + + + + + + + + + 记录上传 + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 读取所有记录 + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 上传记录 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 停止上传 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 删除记录 + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 0 + 200 + + + + 采集&预览 + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 停止预览 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 停止采集 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 开始采集 + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 开始预览 + + + + + + + + + + + 设备测试 + + + + + + + 0 + 300 + + + + 测试指令 + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + 预设配置 + + + + + + + 16777215 + 1 + + + + + + + + + + + + 0 + 30 + + + + 修改信号源为方波 + + + + + + + + 16777215 + 1 + + + + + + + + + + + + 16777215 + 1 + + + + + + + + + + + + 16777215 + 1 + + + + + + + + + + + + 0 + 1 + + + + + 16777215 + 1 + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 1 + 0 + + + + + 0 + 30 + + + + 写ECG寄存器() + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + + 0 + 30 + + + + 读取ECG芯片寄存器 + + + + + + + + 0 + 30 + + + + 批量设置寄存器 + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + 基本操作 + + + + + + + 16777215 + 1 + + + + + + + + + + + + 16777215 + 1 + + + + + + + + + + + + 0 + 30 + + + + 停止采集 + + + + + + + + 0 + 1 + + + + + 16777215 + 1 + + + + + + + + + + + + 0 + 30 + + + + 开始采集 + + + + + + + + 16777215 + 1 + + + + + + + + + + + + 16777215 + 1 + + + + + + + + + + + + + + + 0 + 30 + + + + 写ECG寄存器 + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + + 1 + 0 + + + + + 0 + 0 + + + + + 16777215 + 30 + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 滤波器配置 + + + + + 10 + 10 + 650 + 371 + + + + 滤波器配置 + + + + + + 低通滤波器 + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 1 + 0 + + + + 截至频率(HZ) + + + + + + + + 1 + 0 + + + + + + + + + + + + 1 + 0 + + + + + + + + + + + + 1 + 0 + + + + 使能 + + + + + + + + 1 + 0 + + + + + + + + + 1 + 0 + + + + 阶级 + + + + + + + + + + 陷波滤波器（带阻） + + + + + + + 1 + 0 + + + + 中心频率(HZ) + + + + + + + + 1 + 0 + + + + 窗口宽度(HZ) + + + + + + + + 1 + 0 + + + + + + + + + + + + 1 + 0 + + + + 使能 + + + + + + + + 1 + 0 + + + + + + + + + 1 + 0 + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 1 + 0 + + + + + + + + + 1 + 0 + + + + 阶级 + + + + + + + + + + 高通滤波器 + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 1 + 0 + + + + 截至频率(HZ) + + + + + + + + 1 + 0 + + + + + + + + + + + + 1 + 0 + + + + + + + + + + + + 1 + 0 + + + + 使能 + + + + + + + + 1 + 0 + + + + + + + + + 1 + 0 + + + + 阶级 + + + + + + + + + + + 0 + 150 + + + + 通用参数 + + + + + + + 1 + 0 + + + + + + + + + + + + 1 + 0 + + + + 数据采样周期 + + + + + + + Qt::Horizontal + + + + 300 + 20 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + 540 + 400 + 111 + 41 + + + + 更新滤波器参数 + + + diff --git a/src/algo/iflytop_simple_filter.cpp b/src/algo/iflytop_simple_filter.cpp new file mode 100644 index 0000000..82d9e6b --- /dev/null +++ b/src/algo/iflytop_simple_filter.cpp @@ -0,0 +1,134 @@ + +#include "iflytop_simple_filter.h" + +#include +#include +#include +#include + +#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]); +} \ No newline at end of file diff --git a/src/algo/iflytop_simple_filter.h b/src/algo/iflytop_simple_filter.h new file mode 100644 index 0000000..ea5e602 --- /dev/null +++ b/src/algo/iflytop_simple_filter.h @@ -0,0 +1,72 @@ +#ifndef FILTERS_H +#define FILTERS_H +#include + +// #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 \ No newline at end of file diff --git a/src/filter_algo_mgr.cpp b/src/filter_algo_mgr.cpp new file mode 100644 index 0000000..76e6eb6 --- /dev/null +++ b/src/filter_algo_mgr.cpp @@ -0,0 +1,163 @@ +#include "filter_algo_mgr.hpp" + +#include + +#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 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(¬chfilter[i], v0); + } + + return (int32_t)v0; +} + +void FilterGroup::updateParameter() { + std::lock_guard 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(¬chfilter[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(); } diff --git a/src/filter_algo_mgr.hpp b/src/filter_algo_mgr.hpp new file mode 100644 index 0000000..2fd2ae9 --- /dev/null +++ b/src/filter_algo_mgr.hpp @@ -0,0 +1,156 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 \ No newline at end of file