You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
392 lines
12 KiB
392 lines
12 KiB
#include "zble_module.h"
|
|
#include "zdatachannel_service.h"
|
|
#include "znordic.h"
|
|
//
|
|
|
|
#include <stdarg.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
//
|
|
#include <math.h>
|
|
|
|
#include "app_scheduler.h"
|
|
#include "znordic.h"
|
|
|
|
#if defined(UART_PRESENT)
|
|
#include "nrf_uart.h"
|
|
#endif
|
|
#if defined(UARTE_PRESENT)
|
|
#include "nrf_uarte.h"
|
|
#endif
|
|
|
|
#if 0
|
|
int main() {
|
|
one_conduction_main();
|
|
return 0;
|
|
}
|
|
#else
|
|
#include "nrfx_timer.h"
|
|
|
|
ZDATACHANNEL_DEF(m_zhrs, 2 /*优先级*/, 1 /*client num*/); // 蓝牙服务
|
|
static const nrfx_timer_t m_timer = NRFX_TIMER_INSTANCE(1); /**< Timer used for channel sweeps and tx with duty cycle. */
|
|
#define PI 3.14159265358979323846
|
|
/********************************************************************************************************
|
|
* LOW PASS FILTER
|
|
********************************************************************************************************/
|
|
typedef struct {
|
|
float coef[2];
|
|
float v_out[2];
|
|
} LPFilter;
|
|
|
|
void LPFilter_Init(LPFilter *filter, float cutoffFreqHz, float sampleTimeS);
|
|
float LPFilter_Update(LPFilter *filter, float v_in);
|
|
|
|
/********************************************************************************************************
|
|
* HIGH PASS FILTER
|
|
********************************************************************************************************/
|
|
typedef struct {
|
|
float coef;
|
|
float v_out[2];
|
|
float v_in[2];
|
|
|
|
} HPFilter;
|
|
|
|
void HPFilter_Init(HPFilter *filter, float cutoffFreqHz, float sampleTimeS);
|
|
float HPFilter_Update(HPFilter *filter, float v_in);
|
|
|
|
/********************************************************************************************************
|
|
* BAND PASS FILTER
|
|
********************************************************************************************************/
|
|
|
|
typedef struct {
|
|
LPFilter lpf;
|
|
HPFilter hpf;
|
|
float out_in;
|
|
} PBFilter;
|
|
|
|
void PBFilter_Init(PBFilter *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS);
|
|
float PBFilter_Update(PBFilter *filter, float v_in);
|
|
|
|
/********************************************************************************************************
|
|
* NOTCH FILTER
|
|
********************************************************************************************************/
|
|
|
|
typedef struct {
|
|
float alpha;
|
|
float beta;
|
|
|
|
float vin[3];
|
|
float vout[3];
|
|
|
|
} NOTCHFilter;
|
|
|
|
void NOTCHFilter_Init(NOTCHFilter *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS);
|
|
float NOTCHFilter_Update(NOTCHFilter *filter, float vin);
|
|
|
|
#define PI 3.141592653
|
|
|
|
/********************************************************************************************************
|
|
* LOW PASS FILTER
|
|
********************************************************************************************************/
|
|
|
|
void LPFilter_Init(LPFilter *filter, float cutoffFreqHz, float sampleTimeS) {
|
|
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;
|
|
}
|
|
|
|
float LPFilter_Update(LPFilter *filter, float 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 *filter, float cutoffFreqHz, float sampleTimeS) {
|
|
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;
|
|
}
|
|
|
|
float HPFilter_Update(HPFilter *filter, float 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 *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS) {
|
|
LPFilter_Init(&filter->lpf, LPF_cutoffFreqHz, sampleTimeS);
|
|
HPFilter_Init(&filter->hpf, HPF_cutoffFreqHz, sampleTimeS);
|
|
filter->out_in = 0.0;
|
|
}
|
|
|
|
float PBFilter_Update(PBFilter *filter, float 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 *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS) {
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
float NOTCHFilter_Update(NOTCHFilter *filter, float 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]);
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* MAIN *
|
|
*******************************************************************************/
|
|
|
|
static void zdatachannel_data_handler(zdatachannel_evt_t *p_evt) {
|
|
/**
|
|
* @brief
|
|
*/
|
|
if (p_evt->type == ZDATACHANNEL_EVT_RX_DATA) {
|
|
}
|
|
}
|
|
|
|
static void on_service_init(void) {
|
|
ZLOGI("init zdatachannel service");
|
|
zdatachannel_init_t zdatachannle_init;
|
|
memset(&zdatachannle_init, 0, sizeof(zdatachannle_init));
|
|
zdatachannle_init.data_handler = zdatachannel_data_handler;
|
|
ZERROR_CHECK(zdatachannel_init(&m_zhrs, &zdatachannle_init));
|
|
}
|
|
|
|
int16_t adc_val_cache0[5] = {0};
|
|
int16_t adc_val_cache1[5] = {0};
|
|
int16_t adc_val_cache2[5] = {0};
|
|
int16_t adc_val_cache3[5] = {0};
|
|
int adc_val_index = 0;
|
|
|
|
void sendpacket_to_pc() {
|
|
uint8_t data[255];
|
|
// adc_val_cache[0] = 1;
|
|
// adc_val_cache[1] = 2;
|
|
// adc_val_cache[2] = 3;
|
|
// adc_val_cache[3] = 4;
|
|
// adc_val_cache[4] = 5;
|
|
for (int i = 0; i < adc_val_index; i++) {
|
|
if (adc_val_cache0[i] > 4096) {
|
|
adc_val_cache0[i] = 4096;
|
|
}
|
|
if (adc_val_cache1[i] > 4096) {
|
|
adc_val_cache1[i] = 4096;
|
|
}
|
|
if (adc_val_cache2[i] > 4096) {
|
|
adc_val_cache2[i] = 4096;
|
|
}
|
|
if (adc_val_cache3[i] > 4096) {
|
|
adc_val_cache3[i] = 4096;
|
|
}
|
|
|
|
if (adc_val_cache0[i] < 0) {
|
|
adc_val_cache0[i] = 0;
|
|
}
|
|
if (adc_val_cache1[i] < 0) {
|
|
adc_val_cache1[i] = 0;
|
|
}
|
|
if (adc_val_cache2[i] < 0) {
|
|
adc_val_cache2[i] = 0;
|
|
}
|
|
if (adc_val_cache3[i] < 0) {
|
|
adc_val_cache3[i] = 0;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < adc_val_index; i++) {
|
|
data[i * 12 + 0] = 0xA2;
|
|
data[i * 12 + 1] = 0x2;
|
|
data[i * 12 + 2] = adc_val_cache0[i] & 0xff;
|
|
data[i * 12 + 3] = adc_val_cache0[i] >> 8 & 0xff;
|
|
data[i * 12 + 4] = adc_val_cache1[i] & 0xff;
|
|
data[i * 12 + 5] = adc_val_cache1[i] >> 8 & 0xff;
|
|
data[i * 12 + 6] = adc_val_cache2[i] & 0xff;
|
|
data[i * 12 + 7] = adc_val_cache2[i] >> 8 & 0xff;
|
|
data[i * 12 + 8] = adc_val_cache3[i] & 0xff;
|
|
data[i * 12 + 9] = adc_val_cache3[i] >> 8 & 0xff;
|
|
data[i * 12 + 10] = 0x2;
|
|
data[i * 12 + 11] = 0xA2;
|
|
}
|
|
zdatachannel_data_send2(data, 12 * adc_val_index);
|
|
}
|
|
#if 1
|
|
|
|
#if 0
|
|
// 定义一阶高通滤波器结构体
|
|
typedef struct {
|
|
float alpha; // 时间常数
|
|
float previous_output; // 上一时刻的输出
|
|
} HighPassFilter;
|
|
|
|
HighPassFilter myFilter;
|
|
|
|
// 初始化滤波器
|
|
void initializeFilter(HighPassFilter* filter, float alpha) {
|
|
filter->alpha = alpha;
|
|
filter->previous_output = 0.0;
|
|
}
|
|
|
|
// 一阶高通滤波函数
|
|
float filterValue(HighPassFilter* filter, float input) {
|
|
// 计算输出
|
|
float output = filter->alpha * (input - filter->previous_output) + filter->previous_output;
|
|
|
|
// 更新上一次的输出
|
|
filter->previous_output = output;
|
|
|
|
return output;
|
|
}
|
|
#endif
|
|
|
|
#endif
|
|
|
|
NOTCHFilter notchfilter;
|
|
LPFilter lowpassfilter;
|
|
static void nrfx_timer_event_handler(nrf_timer_event_t event_type, void *p_context) { //
|
|
// raw data
|
|
int16_t val = SingleLeadECG_ecg_plod_get_ecg_val(); //
|
|
adc_val_cache0[adc_val_index] = val;
|
|
|
|
int16_t notchf_val = NOTCHFilter_Update(¬chfilter, val);
|
|
adc_val_cache1[adc_val_index] = notchf_val;
|
|
|
|
int16_t lowpassf_val = LPFilter_Update(&lowpassfilter, notchf_val);
|
|
adc_val_cache2[adc_val_index] = lowpassf_val;
|
|
|
|
// val = low_pass_filter(val);
|
|
/*******************************************************************************
|
|
* 显示数据计算并赋值 *
|
|
*******************************************************************************/
|
|
adc_val_index++;
|
|
if (adc_val_index >= 5) {
|
|
if (zdatachannel_is_connected()) {
|
|
sendpacket_to_pc();
|
|
} else {
|
|
}
|
|
adc_val_index = 0;
|
|
}
|
|
|
|
static int cnt;
|
|
static bool state;
|
|
cnt++;
|
|
if (zdatachannel_is_connected()) {
|
|
SingleLeadECG_led_green_set_state(1);
|
|
} else {
|
|
if (cnt % 20 == 0) {
|
|
SingleLeadECG_led_green_set_state(state);
|
|
state = !state;
|
|
}
|
|
}
|
|
}
|
|
int main() { //
|
|
APP_SCHED_INIT(APP_TIMER_SCHED_EVENT_DATA_SIZE, 20);
|
|
// low_pass_filter_init();
|
|
|
|
// initializeFilter(&myFilter, alpha);
|
|
|
|
NOTCHFilter_Init(¬chfilter, 50, 30, 0.002);
|
|
LPFilter_Init(&lowpassfilter, 200, 0.002);
|
|
|
|
znordic_init();
|
|
NRF_LOG_INFO("compile time :%s", __TIME__);
|
|
|
|
ztm_t tm;
|
|
static zble_module_cfg_t cfg = //
|
|
{
|
|
.deviceName = "OneLeadTest",
|
|
.on_service_init = on_service_init,
|
|
};
|
|
zble_module_init(&cfg);
|
|
SingleLeadECG_adc_module_init();
|
|
SingleLeadECG_led_init();
|
|
SingleLeadECG_led_green_set_state(0);
|
|
|
|
zble_module_start_adv();
|
|
|
|
/*******************************************************************************
|
|
* 定时器初始化 *
|
|
*******************************************************************************/
|
|
/**
|
|
* @brief 初始化定时器
|
|
*/
|
|
nrfx_err_t err;
|
|
nrfx_timer_config_t timer_cfg = {
|
|
.frequency = NRF_TIMER_FREQ_500kHz,
|
|
.mode = NRF_TIMER_MODE_TIMER,
|
|
.bit_width = NRF_TIMER_BIT_WIDTH_24,
|
|
.p_context = NULL,
|
|
.interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
|
|
};
|
|
|
|
err = nrfx_timer_init(&m_timer, &timer_cfg, nrfx_timer_event_handler);
|
|
if (err != NRFX_SUCCESS) {
|
|
NRF_LOG_ERROR("nrfx_timer_init failed with: %d\n", err);
|
|
}
|
|
uint32_t timer_ticks = nrfx_timer_ms_to_ticks(&m_timer, 2); // 500HZ
|
|
nrfx_timer_extended_compare(&m_timer, NRF_TIMER_CC_CHANNEL0, timer_ticks, NRF_TIMER_SHORT_COMPARE0_CLEAR_MASK, true);
|
|
nrfx_timer_enable(&m_timer);
|
|
znordic_loop();
|
|
}
|
|
|
|
#endif
|