十大滤波算法程序大全(Arduino精编无错版)

1、本帖最后由 shenhaiyu 于 2013-11-11 23:03 编辑最近用Arduino做电子秤，为了解决数据的跳变研究了不少滤波算法。网上能找到大把的十大滤波算法帖子，每一篇都不太一样，都号称精编啊，除错啊什么的，可是放到板子里却没一个能正常跑起来的。于是决定自己整理一下这些程序，完美移植到Arduino中。所以大家看到这个帖子的时候，不要怀疑我重复发帖。我的代码都是经过反复试验，复制到Arduino中就能开跑的成品代码，移植到自己的程序中非常方便。而且都仔细研究了各个算法，把错误都修正了的（别的程序连冒泡算法都是溢出的，不信自己找来细看看），所以也算个小原创吧，在别人基础上的原创。转

2、载请注明出处：极客工坊  http:/www.geek-By shenhaiyu 2013-11-011、限幅滤波法（又称程序判断滤波法）2、中位值滤波法3、算术平均滤波法4、递推平均滤波法（又称滑动平均滤波法）5、中位值平均滤波法（又称防脉冲干扰平均滤波法）6、限幅平均滤波法7、一阶滞后滤波法8、加权递推平均滤波法9、消抖滤波法10、限幅消抖滤波法11、新增加 卡尔曼滤波（非扩展卡尔曼），代码在17楼（点击这里）感谢zhangzhe0617分享程序默认对int类型数据进行滤波，如需要对其他类型进行滤波，只需要把程序中所有int替换成long、float或者double即可

3、。1、限幅滤波法（又称程序判断滤波法）ARDUINO 代码复制打印1. /*2. A、名称：限幅滤波法（又称程序判断滤波法）3. B、方法：4.     根据经验判断，确定两次采样允许的最大偏差值（设为A），5.     每次检测到新值时判断：6.     如果本次值与上次值之差<=A，则本次值有效，7.     如果本次值与上次值之差>A，则本次值无效，放弃本次值，用上次值代替本次值。8. C、优点：9.     能有效克服因偶然因

4、素引起的脉冲干扰。10. D、缺点：11.     无法抑制那种周期性的干扰。12.     平滑度差。13. E、整理：shenhaiyu 2013-11-0114. */15.  16. int Filter_Value;17. int Value;18.  19. void setup() 20.   Serial.begin(9600);       / 初始化串口通信21.   randomSeed(analogRead(0)

5、; / 产生随机种子22.   Value = 300;23. 24.  25. void loop() 26.   Filter_Value = Filter();       / 获得滤波器输出值27.   Value = Filter_Value;          / 最近一次有效采样的值，该变量为全局变量28.   Serial.println(Filter_Value); / 串口输出29. &

6、#160; delay(50);30. 31.  32. / 用于随机产生一个300左右的当前值33. int Get_AD() 34.   return random(295, 305);35. 36.  37. / 限幅滤波法（又称程序判断滤波法）38. #define FILTER_A 139. int Filter() 40.   int NewValue;41.   NewValue = Get_AD();42.   if(NewValue - Value) > FIL

7、TER_A) | (Value - NewValue) > FILTER_A)43.     return Value;44.   else45.     return NewValue;46. 2、中位值滤波法ARDUINO 代码复制打印1. /*2. A、名称：中位值滤波法3. B、方法：4.     连续采样N次（N取奇数），把N次采样值按大小排列，5.     取中间值为本次有效值。6. C、优点：7.     能有效克服因偶然因素

8、引起的波动干扰；8.     对温度、液位的变化缓慢的被测参数有良好的滤波效果。9. D、缺点：10.     对流量、速度等快速变化的参数不宜。11. E、整理：shenhaiyu 2013-11-0112. */13.  14. int Filter_Value;15.  16. void setup() 17.   Serial.begin(9600);       / 初始化串口通信18.   randomSeed(analog

9、Read(0); / 产生随机种子19. 20.  21. void loop() 22.   Filter_Value = Filter();       / 获得滤波器输出值23.   Serial.println(Filter_Value); / 串口输出24.   delay(50);25. 26.  27. / 用于随机产生一个300左右的当前值28. int Get_AD() 29.   return random(295, 305);30.

10、31.  32. / 中位值滤波法33. #define FILTER_N 10134. int Filter() 35.   int filter_bufFILTER_N;36.   int i, j;37.   int filter_temp;38.   for(i = 0; i < FILTER_N; i+) 39.     filter_bufi = Get_AD();40.     delay(1);41.   42.  

11、; / 采样值从小到大排列（冒泡法）43.   for(j = 0; j < FILTER_N - 1; j+) 44.     for(i = 0; i < FILTER_N - 1 - j; i+) 45.       if(filter_bufi > filter_bufi + 1) 46.         filter_temp = filter_bufi;47.       &#

12、160; filter_bufi = filter_bufi + 1;48.         filter_bufi + 1 = filter_temp;49.       50.     51.   52.   return filter_buf(FILTER_N - 1) / 2;53. 3、算术平均滤波法ARDUINO 代码复制打印1. /*2. A、名称：算术平均滤波法3. B、方法：4.    &

13、#160;连续取N个采样值进行算术平均运算：5.     N值较大时：信号平滑度较高，但灵敏度较低；6.     N值较小时：信号平滑度较低，但灵敏度较高；7.     N值的选取：一般流量，N=12；压力：N=4。8. C、优点：9.     适用于对一般具有随机干扰的信号进行滤波；10.     这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动。11. D、缺点：12.     对于测量速度较慢或要求

14、数据计算速度较快的实时控制不适用；13.     比较浪费RAM。14. E、整理：shenhaiyu 2013-11-0115. */16.  17. int Filter_Value;18.  19. void setup() 20.   Serial.begin(9600);       / 初始化串口通信21.   randomSeed(analogRead(0); / 产生随机种子22. 23.  24. void loop() 25. 

15、0; Filter_Value = Filter();       / 获得滤波器输出值26.   Serial.println(Filter_Value); / 串口输出27.   delay(50);28. 29.  30. / 用于随机产生一个300左右的当前值31. int Get_AD() 32.   return random(295, 305);33. 34.  35. / 算术平均滤波法36. #define FILTER_N 1237. int F

16、ilter() 38.   int i;39.   int filter_sum = 0;40.   for(i = 0; i < FILTER_N; i+) 41.     filter_sum += Get_AD();42.     delay(1);43.   44.   return (int)(filter_sum / FILTER_N);45. 4、递推平均滤波法（又称滑动平均滤波法）ARDUINO 代码复制打印1. /*2. A、名称：

17、递推平均滤波法（又称滑动平均滤波法）3. B、方法：4.     把连续取得的N个采样值看成一个队列，队列的长度固定为N，5.     每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则），6.     把队列中的N个数据进行算术平均运算，获得新的滤波结果。7.     N值的选取：流量，N=12；压力，N=4；液面，N=4-12；温度，N=1-4。8. C、优点：9.     对周期性干扰有良好的抑制作用，平滑度高；10. &

18、#160;   适用于高频振荡的系统。11. D、缺点：12.     灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差；13.     不易消除由于脉冲干扰所引起的采样值偏差；14.     不适用于脉冲干扰比较严重的场合；15.     比较浪费RAM。16. E、整理：shenhaiyu 2013-11-0117. */18.  19. int Filter_Value;20.  21. void setup() 22.  

19、; Serial.begin(9600);       / 初始化串口通信23.   randomSeed(analogRead(0); / 产生随机种子24. 25.  26. void loop() 27.   Filter_Value = Filter();       / 获得滤波器输出值28.   Serial.println(Filter_Value); / 串口输出29.   delay(50);30.

20、 31.  32. / 用于随机产生一个300左右的当前值33. int Get_AD() 34.   return random(295, 305);35. 36.  37. / 递推平均滤波法（又称滑动平均滤波法）38. #define FILTER_N 1239. int filter_bufFILTER_N + 1;40. int Filter() 41.   int i;42.   int filter_sum = 0;43.   filter_bufFILTER_N = Get_AD

21、();44.   for(i = 0; i < FILTER_N; i+) 45.     filter_bufi = filter_bufi + 1; / 所有数据左移，低位仍掉46.     filter_sum += filter_bufi;47.   48.   return (int)(filter_sum / FILTER_N);49. 5、中位值平均滤波法（又称防脉冲干扰平均滤波法）ARDUINO 代码复制打印1. /*2. A、名称：中位值平均滤波法（又称防脉冲干扰平均滤波

22、法）3. B、方法：4.     采一组队列去掉最大值和最小值后取平均值，5.     相当于“中位值滤波法”+“算术平均滤波法”。6.     连续采样N个数据，去掉一个最大值和一个最小值，7.     然后计算N-2个数据的算术平均值。8.     N值的选取：3-14。9. C、优点：10.     融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。11.     对于偶

23、然出现的脉冲性干扰，可消除由其所引起的采样值偏差。12.     对周期干扰有良好的抑制作用。13.     平滑度高，适于高频振荡的系统。14. D、缺点：15.     计算速度较慢，和算术平均滤波法一样。16.     比较浪费RAM。17. E、整理：shenhaiyu 2013-11-0118. */19.  20. int Filter_Value;21.  22. void setup() 23.   Serial.beg

24、in(9600);       / 初始化串口通信24.   randomSeed(analogRead(0); / 产生随机种子25. 26.  27. void loop() 28.   Filter_Value = Filter();       / 获得滤波器输出值29.   Serial.println(Filter_Value); / 串口输出30.   delay(50);31. 32.  33. /

25、用于随机产生一个300左右的当前值34. int Get_AD() 35.   return random(295, 305);36. 37.  38. / 中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法1）39. #define FILTER_N 10040. int Filter() 41.   int i, j;42.   int filter_temp, filter_sum = 0;43.   int filter_bufFILTER_N;44.   for(i = 0;

26、 i < FILTER_N; i+) 45.     filter_bufi = Get_AD();46.     delay(1);47.   48.   / 采样值从小到大排列（冒泡法）49.   for(j = 0; j < FILTER_N - 1; j+) 50.     for(i = 0; i < FILTER_N - 1 - j; i+) 51.       if(filter_bufi > fi

27、lter_bufi + 1) 52.         filter_temp = filter_bufi;53.         filter_bufi = filter_bufi + 1;54.         filter_bufi + 1 = filter_temp;55.       56.     57.   58.  

28、; / 去除最大最小极值后求平均59.   for(i = 1; i < FILTER_N - 1; i+) filter_sum += filter_bufi;60.   return filter_sum / (FILTER_N - 2);61. 62.  63.  64. /  中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法2）65. /*66. #define FILTER_N 10067. int Filter() 68.   int i;69.   int fil

29、ter_sum = 0;70.   int filter_max, filter_min;71.   int filter_bufFILTER_N;72.   for(i = 0; i < FILTER_N; i+) 73.     filter_bufi = Get_AD();74.     delay(1);75.   76.   filter_max = filter_buf0;77.   filter_min = filter_buf0;78.   f

30、ilter_sum = filter_buf0;79.   for(i = FILTER_N - 1; i > 0; i-) 80.     if(filter_bufi > filter_max)81.       filter_max=filter_bufi;82.     else if(filter_bufi < filter_min)83.       filter_min=filter_bufi;

31、84.     filter_sum = filter_sum + filter_bufi;85.     filter_bufi = filter_bufi - 1;86.   87.   i = FILTER_N - 2;88.   filter_sum = filter_sum - filter_max - filter_min + i / 2; / +i/2 的目的是为了四舍五入89.   filter_sum = filter_sum / i;90.   return fil

32、ter_sum;91. */6、限幅平均滤波法ARDUINO 代码复制打印1. /*2. A、名称：限幅平均滤波法3. B、方法：4.     相当于“限幅滤波法”+“递推平均滤波法”；5.     每次采样到的新数据先进行限幅处理，6.     再送入队列进行递推平均滤波处理。7. C、优点：8.     融合了两种滤波法的优点；9.     对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差。10. D、缺点：11.  

33、   比较浪费RAM。12. E、整理：shenhaiyu 2013-11-0113. */14.  15. #define FILTER_N 1216. int Filter_Value;17. int filter_bufFILTER_N;18.  19. void setup() 20.   Serial.begin(9600);       / 初始化串口通信21.   randomSeed(analogRead(0); / 产生随机种子22.   

34、;filter_bufFILTER_N - 2 = 300;23. 24.  25. void loop() 26.   Filter_Value = Filter();       / 获得滤波器输出值27.   Serial.println(Filter_Value); / 串口输出28.   delay(50);29. 30.  31. / 用于随机产生一个300左右的当前值32. int Get_AD() 33.   return random(2

35、95, 305);34. 35.  36. / 限幅平均滤波法37. #define FILTER_A 138. int Filter() 39.   int i;40.   int filter_sum = 0;41.   filter_bufFILTER_N - 1 = Get_AD();42.   if(filter_bufFILTER_N - 1 - filter_bufFILTER_N - 2) > FILTER_A) | (filter_bufFILTER_N - 2 - filter_

36、bufFILTER_N - 1) > FILTER_A)43.     filter_bufFILTER_N - 1 = filter_bufFILTER_N - 2;44.   for(i = 0; i < FILTER_N - 1; i+) 45.     filter_bufi = filter_bufi + 1;46.     filter_sum += filter_bufi;47.   48.   return (int)filter_sum / (F

37、ILTER_N - 1);49. 7、一阶滞后滤波法ARDUINO 代码复制打印1. /*2. A、名称：一阶滞后滤波法3. B、方法：4.     取a=0-1，本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。5. C、优点：6.     对周期性干扰具有良好的抑制作用；7.     适用于波动频率较高的场合。8. D、缺点：9.     相位滞后，灵敏度低；10.     滞后程度取决于a值大小；11.    &#

38、160;不能消除滤波频率高于采样频率1/2的干扰信号。12. E、整理：shenhaiyu 2013-11-0113. */14.  15. int Filter_Value;16. int Value;17.  18. void setup() 19.   Serial.begin(9600);       / 初始化串口通信20.   randomSeed(analogRead(0); / 产生随机种子21.   Value = 300;22. 23.  24.

39、 void loop() 25.   Filter_Value = Filter();       / 获得滤波器输出值26.   Serial.println(Filter_Value); / 串口输出27.   delay(50);28. 29.  30. / 用于随机产生一个300左右的当前值31. int Get_AD() 32.   return random(295, 305);33. 34.  35. / 一阶滞后滤波法36. #define

40、 FILTER_A 0.0137. int Filter() 38.   int NewValue;39.   NewValue = Get_AD();40.   Value = (int)(float)NewValue * FILTER_A + (1.0 - FILTER_A) * (float)Value);41.   return Value;42. 8、加权递推平均滤波法ARDUINO 代码复制打印1. /*2. A、名称：加权递推平均滤波法3. B、方法：4.     是对递

41、推平均滤波法的改进，即不同时刻的数据加以不同的权；5.     通常是，越接近现时刻的数据，权取得越大。6.     给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低。7. C、优点：8.     适用于有较大纯滞后时间常数的对象，和采样周期较短的系统。9. D、缺点：10.     对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号；11.     不能迅速反应系统当前所受干扰的严重程度，滤波效果差。12. E、整理：shenhaiy

42、u 2013-11-0113. */14.  15. int Filter_Value;16.  17. void setup() 18.   Serial.begin(9600);       / 初始化串口通信19.   randomSeed(analogRead(0); / 产生随机种子20. 21.  22. void loop() 23.   Filter_Value = Filter();       / 获得滤波

43、器输出值24.   Serial.println(Filter_Value); / 串口输出25.   delay(50);26. 27.  28. / 用于随机产生一个300左右的当前值29. int Get_AD() 30.   return random(295, 305);31. 32.  33. / 加权递推平均滤波法34. #define FILTER_N 1235. int coeFILTER_N = 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;   

44、; / 加权系数表36. int sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12; / 加权系数和37. int filter_bufFILTER_N + 1;38. int Filter() 39.   int i;40.   int filter_sum = 0;41.   filter_bufFILTER_N = Get_AD();42.   for(i = 0; i < FILTER_N; i+) 43.    

45、 filter_bufi = filter_bufi + 1; / 所有数据左移，低位仍掉44.     filter_sum += filter_bufi * coei;45.   46.   filter_sum /= sum_coe;47.   return filter_sum;48. 9、消抖滤波法ARDUINO 代码复制打印1. /*2. A、名称：消抖滤波法3. B、方法：4.     设置一个滤波计数器，将每次采样值与当前有效值比较：5.    &#

46、160;如果采样值=当前有效值，则计数器清零；6.     如果采样值<>当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）；7.     如果计数器溢出，则将本次值替换当前有效值，并清计数器。8. C、优点：9.     对于变化缓慢的被测参数有较好的滤波效果；10.     可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。11. D、缺点：12.     对于快速变化的参数不宜；13.  &#

47、160;  如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。14. E、整理：shenhaiyu 2013-11-0115. */16.  17. int Filter_Value;18. int Value;19.  20. void setup() 21.   Serial.begin(9600);       / 初始化串口通信22.   randomSeed(analogRead(0); / 产生随机种子23.   Value

48、 = 300;24. 25.  26. void loop() 27.   Filter_Value = Filter();       / 获得滤波器输出值28.   Serial.println(Filter_Value); / 串口输出29.   delay(50);30. 31.  32. / 用于随机产生一个300左右的当前值33. int Get_AD() 34.   return random(295, 305);35. 36.  

49、37. / 消抖滤波法38. #define FILTER_N 1239. int i = 0;40. int Filter() 41.   int new_value;42.   new_value = Get_AD();43.   if(Value != new_value) 44.     i+;45.     if(i > FILTER_N) 46.       i = 0;47.       Valu

50、e = new_value;48.     49.   50.   else51.     i = 0;52.   return Value;53. 10、限幅消抖滤波法ARDUINO 代码复制打印1. /*2. A、名称：限幅消抖滤波法3. B、方法：4.     相当于“限幅滤波法”+“消抖滤波法”；5.     先限幅，后消抖。6. C、优点：7.     继承了“限幅”和“消抖”的优点；8.

51、60;   改进了“消抖滤波法”中的某些缺陷，避免将干扰值导入系统。9. D、缺点：10.     对于快速变化的参数不宜。11. E、整理：shenhaiyu 2013-11-0112. */13.  14. int Filter_Value;15. int Value;16.  17. void setup() 18.   Serial.begin(9600);       / 初始化串口通信19.   randomSeed(analogRe

52、ad(0); / 产生随机种子20.   Value = 300;21. 22.  23. void loop() 24.   Filter_Value = Filter();       / 获得滤波器输出值25.   Serial.println(Filter_Value); / 串口输出26.   delay(50);27. 28.  29. / 用于随机产生一个300左右的当前值30. int Get_AD() 31.   re

53、turn random(295, 305);32. 33.  34. / 限幅消抖滤波法35. #define FILTER_A 136. #define FILTER_N 537. int i = 0;38. int Filter() 39.   int NewValue;40.   int new_value;41.   NewValue = Get_AD();42.   if(NewValue - Value) > FILTER_A) | (Value - NewValue) > FIL

54、TER_A)43.     new_value = Value;44.   else45.     new_value = NewValue;46.   if(Value != new_value) 47.     i+;48.     if(i > FILTER_N) 49.       i = 0;50.       Value = new_value;51.    

55、 52.   53.   else54.     i = 0;55.   return Value;56. 建议编辑一下这个帖子作为滤波专用的，这样大家查起来也方便。下面是卡尔曼滤波，不是扩展的，但是输出平稳的俯仰和滚转应该够了(凑乎用吧我也不是专业写代码的，欢迎大家拍)1. #include <Wire.h> / I2C library, gyroscope2.3. / Accelerometer ADXL3454. #define ACC (0x53)    /ADXL345 A

56、CC address5. #define A_TO_READ (6)        /num of bytes we are going to read each time (two bytes for each axis)6.7.8. / Gyroscope ITG3200 9. #define GYRO 0x68 / gyro address, binary = 11101000 when AD0 is connected to Vcc (see schematics of your breakout board)10.

57、 #define G_SMPLRT_DIV 0x15   11. #define G_DLPF_FS 0x16   12. #define G_INT_CFG 0x1713. #define G_PWR_MGM 0x3E14.15. #define G_TO_READ 8 / 2 bytes for each axis x, y, z16.17.18. / offsets are chip specific. 19. int a_offx = 0;20. int a_offy = 0;21. int a_offz = 0;22.23. int

58、g_offx = 0;24. int g_offy = 0;25. int g_offz = 0;26. /27.28. /29. char str512; 30.31. void initAcc() 32.   /Turning on the ADXL34533.   writeTo(ACC, 0x2D, 0);      34.   writeTo(ACC, 0x2D, 16);35.   writeTo(ACC, 0x2D, 8);36.  

59、0;/by default the device is in +-2g range reading37. 38.39. void getAccelerometerData(int* result) 40.   int regAddress = 0x32;    /first axis-acceleration-data register on the ADXL34541.   byte buffA_TO_READ;42.   43.   readFrom(ACC, regAddress, A_T

60、O_READ, buff); /read the acceleration data from the ADXL34544.   45.   /each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!46.   /thus we are converting both bytes in to one int47.   result0 = (int)buff1) <<

61、8) | buff0 + a_offx;   48.   result1 = (int)buff3) << 8) | buff2 + a_offy;49.   result2 = (int)buff5) << 8) | buff4 + a_offz;50. 51.52. /initializes the gyroscope53. void initGyro()54. 55.   /*56.   * ITG 320057.   * power manag

62、ement set to:58.   * clock select = internal oscillator59.   *     no reset, no sleep mode60.   *   no standby mode61.   * sample rate to = 125Hz62.   * parameter to +/- 2000 degrees/sec63.   * low pass filter

63、= 5Hz64.   * no interrupt65.   */66.   writeTo(GYRO, G_PWR_MGM, 0x00);67.   writeTo(GYRO, G_SMPLRT_DIV, 0x07); / EB, 50, 80, 7F, DE, 23, 20, FF68.   writeTo(GYRO, G_DLPF_FS, 0x1E); / +/- 2000 dgrs/sec, 1KHz, 1E, 1969.   writeTo(GYRO, G_INT_

64、CFG, 0x00);70. 71.72.73. void getGyroscopeData(int * result)74. 75.   /*76.   Gyro ITG-3200 I2C77.   registers:78.   temp MSB = 1B, temp LSB = 1C79.   x axis MSB = 1D, x axis LSB = 1E80.   y axis MSB = 1F, y axis LSB = 2081.   z a

65、xis MSB = 21, z axis LSB = 2282.   */83.84.   int regAddress = 0x1B;85.   int temp, x, y, z;86.   byte buffG_TO_READ;87.   88.   readFrom(GYRO, regAddress, G_TO_READ, buff); /read the gyro data from the ITG320089.   90.  

66、0;result0 = (buff2 << 8) | buff3) + g_offx;91.   result1 = (buff4 << 8) | buff5) + g_offy;92.   result2 = (buff6 << 8) | buff7) + g_offz;93.   result3 = (buff0 << 8) | buff1; / temperature94.   95. 96.97.98. float xz=0,yx=0,yz=0;99. flo

67、at p_xz=1,p_yx=1,p_yz=1;100. float q_xz=0.0025,q_yx=0.0025,q_yz=0.0025;101. float k_xz=0,k_yx=0,k_yz=0;102. float r_xz=0.25,r_yx=0.25,r_yz=0.25;103.   /int acc_temp3;104.   /float acc3;105.   int acc3;106.   int gyro4;107.   float Axz;108.  

68、0;float Ayx;109.   float Ayz;110.   float t=0.025;111. void setup()112. 113.   Serial.begin(9600);114.   Wire.begin();115.   initAcc();116.   initGyro();117.   118. 119.120. /unsigned long timer = 0;121. /float o;122. void loop()1

69、23. 124.   125.   getAccelerometerData(acc);126.   getGyroscopeData(gyro);127.   /timer = millis();128.   sprintf(str, "%d,%d,%d,%d,%d,%d", acc0,acc1,acc2,gyro0,gyro1,gyro2);129.   130.   /acc0=acc0;131.   /acc2=acc2;132.   /acc1=acc1;133.   /r=sqrt(acc0*acc0+acc1*acc1+acc2*acc2);134.   gyro0=gyro0/ 14.375;135.   gy