filter.c

基于陀螺仪原理的惯性导航程序 用于测量量角速度

/**********************************************************************
  filter.c file
  作者：lig&lij
  建立日期：2006-4-25
  修改日期：2006-4-25
  版本：V1.0
**********************************************************************/

#include <ADuC7020.H>
#include"stdio.h"
#include "my_type.h"
#include "LED.H"
#include "interrupt.h"
#include "Timer.h"
#include "UART.H"
#include "ADC.h"
#include "others.h"

#define LEN  6   //移动算术平均的个数+2=SHIFT<<2+2
#define SHIFT 2   //2^SHIFT
///////////////////////////////////////////////////
//extern  uint16  AD_value[5];	 

///////////////////////////////////////////////////
uint8 pdata;    //移动指针
uint8 pmax,pmin;   //记录数据表中最大值和最小值的位置,
       //在一般的数据采集系统中,数据的长度>=8,
       //因此用指针记录而不是直接记录最大值和最小值
uint16 datas[LEN];

uint16 szlb(uint16 _data)
{  
/****************************/
/* 在调用此子程序前必须对 */
/* pdata,datas[]数组,  */
/* pmax,pmin进行初始化  */
/****************************/

 uint8 i;
 uint32 average1=0;
 uint16 average=0;  //清零，用来计算平均值
 pdata=(pdata+1)%LEN; //指针下标在0到LEN-1上滑动
 datas[pdata]=_data;  //采样所得数据存入数据表中
 for(i=0;i<LEN;i++)
         average1+=datas[i]; //求所有数据总和

/*******去除被认为是脉冲的数据******/
 if(_data>datas[pmax])
      pmax=pdata;   //得到最大值的指针
 else if(_data<datas[pmin])
      pmin=pdata;   //得到最小值的指针
 if(pdata==pmax)   //如果当前输入值将存入当前最大值的位置时
 {      //由以上方法将不可行，必须从其他位置中查找极值
      for(i=0;i<LEN;i++)
          if(datas[i]>datas[pmax])
          pmax=i;
 }
 else if(pdata==pmin)//如果当前输入值将存入当前最大值的位置时
 {      //由以上方法将不可行，必须从其他位置中查找极值
     for(i=0;i<LEN;i++)
          if(datas[i]<datas[pmin])
           pmin=i;
 }
 average=average1-datas[pmax]-datas[pmin];//减去脉冲

 return (average>>SHIFT);    //求算术平均值
}