STM32F103 数字滤波的编码器测速

工作中经常遇到需要连续测速的应用，担心编码器“抖动”，导致速度测不准

一. 简单原理

      编码器计数  ---->  数值取样  ----> 差分  ----> FIR滤波  ---->  比例计算实际速度

二. 硬件接口

  

 

三. 差分计算

  

#define MAXCOUNT1  		20000		//最大计数值 per 0.1s
#define ENCODER_TIM_PERIOD 	39999		//计数自动重装值 值在 0 到 39999之间
/*使用100ms 定时器，定时调用该函数*/
int16_t Enc_GetCount(void)
{
	static uint16_t last_count = 0;
	uint16_t cur_count;

	cur_count = __HAL_TIM_GetCounter(&htim2);      /*获取编码计数值*/
	
	int32_t Angle = cur_count - last_count;
	
	if(Angle > MAXCOUNT1)                          /*判定上溢出*/
	{
		Angle -= ENCODER_TIM_PERIOD;
	}
	if(Angle < -MAXCOUNT1)                         /*判定下溢出*/
	{
		Angle += ENCODER_TIM_PERIOD;
	}
	
	last_count = cur_count;
	
	return Angle;                                  /*返回差分值*/
}

四. FIR滤波器

  使用matlab 的 FDAtool工具，设计FIR滤波器

采样频率Fs = 10Hz  阶数order = 5(过长延迟增加)

导出滤波器参数

h(6) =

   -0.0691    0.0296    0.5204    0.5204    0.0296   -0.0691

C语言实现的FIR滤波器计算

/*简单的FIR滤波器*/
const float h[6] = {-0.0691  ,0.0296 ,0.5204 ,0.5204 ,0.0296 ,-0.0691}
float Fir_Filter(int16_t xn)
{
    static int16_t xx[6] = {0};        /*FIR 长度6*/
    float ret;
    int16_t i;

    xx[0] = xn;
    
    ret = xx[0] * h[0];

    for(i = 5;i > 0;i--)
    {
        ret += h(i) * xx[i];
        xx(i) = xx(i-1);
    }
    
    return ret;
}

5. 计算速度

    结合编码器一圈的脉冲数，可以换算成速度   如角频率，圈每分，圈每秒等