|
|
@ -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(); |
|
|
|