diff --git a/lark1fq/src/filter.c b/lark1fq/src/filter.c index 2b4489f..8a91393 100755 --- a/lark1fq/src/filter.c +++ b/lark1fq/src/filter.c @@ -74,25 +74,31 @@ void filter_init(void) void filter_quadratic_process(void) { + static float peak[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + static float rative[4] = {0.0f, 0.0f, 0.0f, 0.0f}; struct quadratic_coefficient_s coeff; - float rative, peak; - uint32_t pos_ravine, pos_peak; + float result_fit; + uint32_t position; - pos_ravine = (data_ltc1867.sample_falling + data_ltc1867.sample_rising - CALC_POINTS) / 2; - pos_peak = (data_ltc1867.sample_falling - CALC_POINTS) / 2; + position = (data_ltc1867.sample_total - CALC_POINTS) / 2; for (uint8_t i = 0; i < 4; i++) { - calc_quadratic_fit(data_ltc1867.p_calc[i] + pos_ravine, &coeff, CALC_POINTS); - rative = coeff.c - coeff.b * coeff.b / (4.0f * coeff.a); - calc_quadratic_fit(data_ltc1867.p_calc[i] + pos_peak, &coeff, CALC_POINTS); - peak = coeff.c - coeff.b * coeff.b / (4.0f * coeff.a); - data_filter[i].raw_f = peak - rative; + calc_quadratic_fit(data_ltc1867.p_calc[i] + position, &coeff, CALC_POINTS); + result_fit = coeff.c - coeff.b * coeff.b / (4.0f * coeff.a); + if (result_fit > 32768.0f && result_fit < 65536.0f) { + peak[i] = result_fit; + } else if (result_fit < 32768.0f && result_fit > 0.0f) { + rative[i] = result_fit; + } else { + return; + } + data_filter[i].raw_f = peak[i] - rative[i]; debug_write_data(DEBUG_CH_QUADRATIC + i, data_filter[i].raw_f); if (data_filter[i].raw_f < 0.0f) { data_filter[i].raw_u = 0; } else if (data_filter[i].raw_f >= 65535.0f) { - data_filter[i].raw_u = 65535; + data_filter[i].raw_u = 655360; } else { - data_filter[i].raw_u = (uint32_t)(data_filter[i].raw_f); + data_filter[i].raw_u = (uint32_t)(data_filter[i].raw_f * 10.0f); } } } @@ -169,6 +175,13 @@ void filter_output_process(void) { if (flag.filter_climb > 0) { flag.filter_climb--; + if (system_tick_cnt > 25 * 1000) { + while (flag.filter_climb) { + filter_median_process(); + filter_window_process(); + flag.filter_climb--; + } + } return; } for (uint8_t i = 0; i < 4; i++) { diff --git a/lark1fq/src/filter.h b/lark1fq/src/filter.h index 234c672..5a921c6 100755 --- a/lark1fq/src/filter.h +++ b/lark1fq/src/filter.h @@ -40,6 +40,8 @@ struct data_filter_s { uint32_t window_output; }; +extern uint32_t system_tick_cnt; + extern void filter_init(void); extern void filter_quadratic_process(void); extern void filter_median_process(void); diff --git a/lark1fq/src/ltc1867.c b/lark1fq/src/ltc1867.c index a468c93..c4706ca 100755 --- a/lark1fq/src/ltc1867.c +++ b/lark1fq/src/ltc1867.c @@ -10,7 +10,7 @@ #include "stm32f4xx_tim.h" #include "stm32f4xx_spi.h" -#define LTC1867_RAW_BUFF_SIZE (SAMPLES_PER_PERIOD / 4 * 5) +#define LTC1867_RAW_BUFF_SIZE (SAMPLES_PER_PERIOD / 4 * 3) float data_ltc1867_raw[2][4][LTC1867_RAW_BUFF_SIZE] __attribute__((section(".bss_ccm"))); struct data_ltc1867_s data_ltc1867 __attribute__((section(".bss_ccm"))); __attribute__((section(".data_ccm"))) uint16_t ltc1867_spi_data[LTC1867_CH_CNT] = {0xC400, 0x9400, 0xD400, 0xA400, 0x8400}; /* order: ch1, ch2, ch3, ch4, ch0 */ @@ -209,14 +209,9 @@ void TIM2_IRQHandler(void) } /* search point of 32768 */ - if ((pre_ref < 32768) && (val[0] >= 32768)) { - data_ltc1867.calc_cnt++; - data_ltc1867.sample_rising = data_ltc1867.sample_cnt; - data_ltc1867.sample_cnt = 0; - if ((data_ltc1867.sample_falling > SAMPLES_PER_PERIOD / 16 * 7) && \ - (data_ltc1867.sample_falling < SAMPLES_PER_PERIOD / 16 * 9) && \ - (data_ltc1867.sample_rising > SAMPLES_PER_PERIOD / 16 * 15) && \ - (data_ltc1867.sample_rising < SAMPLES_PER_PERIOD / 16 * 17)) { + if (((pre_ref < 32768) && (val[0] >= 32768)) || ((pre_ref >= 32768) && (val[0] < 32768))) { + if ((data_ltc1867.sample_cnt > SAMPLES_PER_PERIOD / 32 * 15) && \ + (data_ltc1867.sample_cnt < SAMPLES_PER_PERIOD / 32 * 17)) { /* exchange pointor of calc/samp data */ for (uint8_t i = 0; i < 4; i++) { p = data_ltc1867.p_calc[i]; @@ -225,8 +220,9 @@ void TIM2_IRQHandler(void) } data_ltc1867.calc_flag++; /* trig calc */ } - } else if ((pre_ref >= 32768) && (val[0] < 32768)) { - data_ltc1867.sample_falling = data_ltc1867.sample_cnt; + data_ltc1867.calc_cnt++; + data_ltc1867.sample_total = data_ltc1867.sample_cnt; + data_ltc1867.sample_cnt = 0; } pre_ref = val[0]; diff --git a/lark1fq/src/ltc1867.h b/lark1fq/src/ltc1867.h index e53c9b8..0da83bb 100755 --- a/lark1fq/src/ltc1867.h +++ b/lark1fq/src/ltc1867.h @@ -29,8 +29,7 @@ struct data_ltc1867_s { uint32_t calc_cnt; /* totle count of double period complete */ uint16_t ledir_cnt; /* index of led count, for togglr IR led */ uint16_t sample_cnt; /* index of sample */ - uint16_t sample_falling; /* position of falling edge */ - uint16_t sample_rising; /* position of rising edge, is also total sample count */ + uint16_t sample_total; /* total number of sampling data */ float *p_calc[4]; /* pointor for calculation */ float *p_samp[4]; /* pointor for store sample data */ };