base_lib
/
component
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复滑动滤波器

This commit is contained in:
许晟昊 2025-04-30 15:36:20 +08:00
parent 609d10f12f
commit 0ae6f17ecd
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/filter.c
View File

@ -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;
}