陀螺仪raw data 滤波

bruceteen

每五笔做个平均
第一次数值是前五笔(1~5)数据平均值
第二次数值是2~7 数据平均值       ------ 按照规律，应该是 2-6 吧
第三次数值是3~8 数据平均值       ------ 按照规律，应该是 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. }

复制代码