加入平均滤波器功能

Signed-off-by: kevin <huhui@sharkgulf.com>

Applications/math/fast_math.c

@@ -4,6 +4,30 @@
 #include <arm_math.h>
 #include "math/fast_math.h"
 #include "os/os_types.h"
+
+void avg_filter_init(avg_filter_t *f, int len) {
+	f->len = len;
+	f->idx = f->sum = 0;
+	f->buff = os_alloc(len * sizeof(float));
+	if (f->buff == NULL) {
+		return;
+	}
+	for (int i = 0; i < len; i++) {
+		f->buff[i] = 0;
+	}
+}
+
+float avg_filter_do(avg_filter_t *f, float sample) {
+	if (f->buff == NULL) {
+		return 0;
+	}
+	f->sum -= f->buff[f->idx];
+	f->buff[f->idx] = sample;
+	f->sum += f->buff[f->idx];
+	f->idx = (f->idx + 1) % f->len;
+	return f->sum/f->len;
+}
+
 #if 0
 /*
 生成sin/cos 查找表，(0-90°，间隔0.1°)

Applications/math/fast_math.h

@@ -171,5 +171,17 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 /* 后向差分离散化 */
 #define do_lpf(value, sample, filter_constant)	((sample * filter_constant + value)/(1.0f + filter_constant))
 
+/**
+ * A simple avarage filter
+*/
+typedef struct {
+	int len;
+	float *buff;
+	int idx;
+	int sum;
+}avg_filter_t;
+void avg_filter_init(avg_filter_t *f, int len);
+float avg_filter_do(avg_filter_t *f, float sample);
+
 #endif /* _Fast_Math_H__ */