diff --git a/src/filter.c b/src/filter.c index 1a907ce..c92ffd0 100644 --- a/src/filter.c +++ b/src/filter.c @@ -57,6 +57,7 @@ float32 kalman_update(kalman_t *cfg, float32 input) // 一阶滞后滤波法 void lpf_init(lpf_t *cfg) { + DBG_ASSERT(cfg != NULL __DBG_LINE); cfg->fisrt_flag = TRUE; cfg->last_value = 0; if (cfg->alpha <= 0 || cfg->alpha > 1) @@ -67,6 +68,7 @@ void lpf_init(lpf_t *cfg) float32 lpf_update(lpf_t *cfg, float32 input) { + DBG_ASSERT(cfg != NULL __DBG_LINE); float32 out; /***************** 如果第一次进入,则给 out_last 赋值 ******************/ @@ -85,6 +87,7 @@ float32 lpf_update(lpf_t *cfg, float32 input) void lpf_reset(lpf_t *cfg) { + DBG_ASSERT(cfg != NULL __DBG_LINE); cfg->fisrt_flag = TRUE; } @@ -94,6 +97,7 @@ void lpf_reset(lpf_t *cfg) void lpf_window_init(lpf_window_t *cfg, uint16_t size) { DBG_ASSERT(cfg != NULL __DBG_LINE); + DBG_ASSERT(size != 0 __DBG_LINE); if (cfg->window != NULL) { osel_mem_free(cfg->window); @@ -108,13 +112,10 @@ void lpf_window_init(lpf_window_t *cfg, uint16_t size) void lpf_window_dinit(lpf_window_t *cfg) { - if (cfg != NULL) + DBG_ASSERT(cfg != NULL __DBG_LINE); + if (cfg->window != NULL) { - if (cfg->window != NULL) - { - osel_mem_free(cfg->window); - } - osel_mem_free(cfg); + osel_mem_free(cfg->window); } } @@ -122,6 +123,8 @@ void lpf_window_dinit(lpf_window_t *cfg) void lpf_window_reset(lpf_window_t *cfg) { DBG_ASSERT(cfg != NULL __DBG_LINE); + DBG_ASSERT(cfg->window != NULL __DBG_LINE); + DBG_ASSERT(cfg->size != 0 __DBG_LINE); cfg->index = 0; cfg->sum = 0; osel_memset((uint8_t *)cfg->window, 0, sizeof(float32) * cfg->size); @@ -130,6 +133,8 @@ void lpf_window_reset(lpf_window_t *cfg) float32 lpf_window_update(lpf_window_t *cfg, float32 input) { DBG_ASSERT(cfg != NULL __DBG_LINE); + DBG_ASSERT(cfg->window != NULL __DBG_LINE); + DBG_ASSERT(cfg->size != 0 __DBG_LINE); cfg->sum = 0; // 如果窗口未满,直接添加新值到当前索引位置 if (cfg->index < cfg->size) @@ -151,6 +156,7 @@ float32 lpf_window_update(lpf_window_t *cfg, float32 input) cfg->sum += cfg->window[i]; } // 计算平均值 + DBG_ASSERT(cfg->index != 0 __DBG_LINE); cfg->out = cfg->sum / cfg->index; return cfg->out; }