diff --git a/app/main.c b/app/main.c index fa049a6..bad3b03 100644 --- a/app/main.c +++ b/app/main.c @@ -30,8 +30,174 @@ int main() { 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; -static void zdatachannel_data_handler(zdatachannel_evt_t* p_evt) { +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 */ @@ -47,8 +213,11 @@ static void on_service_init(void) { ZERROR_CHECK(zdatachannel_init(&m_zhrs, &zdatachannle_init)); } -uint16_t adc_val_cache[5] = {0}; -int adc_val_index = 0; +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]; @@ -57,24 +226,98 @@ void sendpacket_to_pc() { // 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 * 6 + 0] = 0xA2; - data[i * 6 + 1] = 0x2; - data[i * 6 + 2] = adc_val_cache[i] & 0xff; - data[i * 6 + 3] = adc_val_cache[i] >> 8 & 0xff; - data[i * 6 + 4] = 0x2; - data[i * 6 + 5] = 0xA2; + 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, 6 * adc_val_index); + 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 -static void nrfx_timer_event_handler(nrf_timer_event_t event_type, void* p_context) { // - uint16_t val = SingleLeadECG_ecg_plod_get_ecg_val(); // 12bit +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_cache[adc_val_index] = val; adc_val_index++; if (adc_val_index >= 5) { if (zdatachannel_is_connected()) { @@ -90,7 +333,7 @@ static void nrfx_timer_event_handler(nrf_timer_event_t event_type, void* p_conte if (zdatachannel_is_connected()) { SingleLeadECG_led_green_set_state(1); } else { - if (cnt % 500 == 0) { + if (cnt % 20 == 0) { SingleLeadECG_led_green_set_state(state); state = !state; } @@ -98,6 +341,12 @@ static void nrfx_timer_event_handler(nrf_timer_event_t event_type, void* p_conte } 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__); @@ -134,7 +383,7 @@ int main() { // 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, 5); // 500HZ + 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();