陀螺儀raw data 濾波

shihyu

我想把 陀螺儀raw data 做濾波 , 每五筆做個(gè)平均
陀螺儀raw data 每筆數(shù)據(jù)有 x , y, z 三個(gè)軸的數(shù)值

第一次數(shù)值是前五筆(1~5)數(shù)據(jù)平均值
第二次數(shù)值是2~7 數(shù)據(jù)平均值
第三次數(shù)值是3~8 數(shù)據(jù)平均值
之后依此類推

陀螺儀raw data 是不斷更新進(jìn)來(lái) , 五筆不斷更新搬動(dòng)數(shù)據(jù) , 如果用 array 存放搬動(dòng)數(shù)據(jù)感覺效率 , 會(huì)是用 linked list

因?yàn)榇�碼要放在 stm32 板子上所以需要考量到效率問題 , 不知道怎么更新存放這些數(shù)據(jù)效率比較好？

xinglp

Round Robin

bruceteen

每五筆做個(gè)平均
第一次數(shù)值是前五筆(1~5)數(shù)據(jù)平均值
第二次數(shù)值是2~7 數(shù)據(jù)平均值       ------ 按照規(guī)律，應(yīng)該是 2-6 吧
第三次數(shù)值是3~8 數(shù)據(jù)平均值       ------ 按照規(guī)律，應(yīng)該是 3-7 吧

1. #include <stdio.h>
   
2. #include <string.h>
   
3. 
   
4. struct rawdata_buf
   
5. {
   
6.         double x[5], y[5], z[5];
   
7.         double sum[3];
   
8.         size_t index;
   
9. };
   
10. void rawdata_buf_append( struct rawdata_buf* raw, double x, double y, double z )
    
11. {
    
12.         raw->sum[0] += -raw->x[raw->index]+x;
    
13.         raw->sum[1] += -raw->y[raw->index]+y;
    
14.         raw->sum[2] += -raw->z[raw->index]+z;
    
15.         raw->x[raw->index] = x;
    
16.         raw->y[raw->index] = y;
    
17.         raw->z[raw->index] = z;
    
18.         raw->index = (raw->index+1)%5;
    
19. }
    
20. void rawdata_buf_average( struct rawdata_buf* raw, double* x, double* y, double* z )
    
21. {
    
22. #ifdef _DEBUG
    
23.         for( size_t i=raw->index; i!=raw->index+5; ++i )
    
24.                 printf( "(%g,%g,%g)", raw->x[i%5], raw->y[i%5], raw->z[i%5] );
    
25.         putchar( '\n' );
    
26. #endif
    
27. 
    
28.         *x = raw->sum[0]/5;
    
29.         *y = raw->sum[1]/5;
    
30.         *z = raw->sum[2]/5;
    
31. }
    
32. 
    
33. int main( void )
    
34. {
    
35.         double avgx, avgy, avgz;
    
36. 
    
37.         struct rawdata_buf buf = { 0 };
    
38.         rawdata_buf_append( &buf, 0,0,0 );
    
39.         rawdata_buf_append( &buf, 1,1,1 );
    
40.         rawdata_buf_append( &buf, 2,2,2 );
    
41.         rawdata_buf_append( &buf, 3,3,3 );
    
42.         rawdata_buf_append( &buf, 4,4,4 );
    
43.         rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
    
44.         printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
    
45. 
    
46.         rawdata_buf_append( &buf, 5,5,5 );
    
47.         rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
    
48.         printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
    
49. 
    
50.         rawdata_buf_append( &buf, 6,6,6 );
    
51.         rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
    
52.         printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
    
53. 
    
54.         rawdata_buf_append( &buf, 7,7,7 );
    
55.         rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
    
56.         printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
    
57. 
    
58.         rawdata_buf_append( &buf, 8,8,8 );
    
59.         rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
    
60.         printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
    
61. 
    
62.         rawdata_buf_append( &buf, 9,9,9 );
    
63.         rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
    
64.         printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
    
65. 
    
66.         return 0;
    
67. }

復(fù)制代碼

shihyu

五筆是最新五筆 ，太舊數(shù)據(jù)就捨棄

karma303

路過幫頂～～～

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. }

復(fù)制代碼