Browse Source

update

test_filter
zhaohe 1 year ago
parent
commit
18e3eff7de
  1. 279
      app/main.c

279
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(&notchfilter, 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(&notchfilter, 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();

Loading…
Cancel
Save