陀螺仪raw data 滤波

cokeboL

1. float calc(float a[], int size, int newNum)
   
2. {
   
3.         static bool inited = false;
   
4.         static float sum = 0;
   
5.         static int idx = 0;
   
6. 
   
7.         if(!inited)
   
8.         {
   
9.                 for(int i = 0; i < size; i++)
   
10.                 {
    
11.                         sum += a[i];
    
12.                 }
    
13.                 inited = true;
    
14.         }
    
15.         else
    
16.         {
    
17.                 sum -= (a[idx] - newNum);
    
18.                 a[idx++] = newNum;
    
19.                 idx %= size;
    
20.         }
    
21. 
    
22.         return sum / size;
    
23. }

复制代码