传感器返回的数据里面有用数据的频率都比较滴，而噪声信号的频率普遍都 比较高。所以可以通过设计一个低通滤波器来对数据进行滤波。
- #ifndef _LPF_FIRST_ORDER_H
- #define _LPF_FIRST_ORDER_H
- typedef struct _lpf_first_order
- {
- float fc; // cut-off frequency
- float uk;
- float alpha; // filter coefficient
- float T; // samping period
- }LpfFirstOderObj;
- void lpf_first_order_init(LpfFirstOderObj *filter, float alpha);
- float lpf_first_order(LpfFirstOderObj *filter, float k);
- void lpf_first_order_fc_set(LpfFirstOderObj *filter, float fc, float T);
- #endif
- #include "lpf_first_order.h"
- #define LPF_PI 3.1415926
- void lpf_first_order_init(LpfFirstOderObj *filter, float alpha)
- {
- filter->alpha = alpha;
- filter->fc = 0;
- filter->T = 0;
- filter->uk = 0;
- }
- float lpf_first_order(LpfFirstOderObj *filter, float k)
- {
- float uo;
- uo = filter->alpha * k + (1 - filter->alpha) * filter->uk;
- filter->uk = uo;
- return uo;
- }
- void lpf_first_order_fc_set(LpfFirstOderObj *filter, float fc, float T)
- {
- filter->fc = fc;
- filter->T = T;
- filter->alpha = (2 * LPF_PI * fc * T) / (1 + 2 * LPF_PI * fc * T);
- }
| 
楼主
