Diff: CircleBuffer.cpp

Revision:

1:8c7e5801d763

Parent:

0:410ebbfd5e14

--- a/CircleBuffer.cpp	Tue Oct 15 16:32:18 2013 +0000
+++ b/CircleBuffer.cpp	Thu Oct 17 03:58:15 2013 +0000
@@ -1,7 +1,5 @@
 #include "CircleBuffer.h"
 
-
-
 CircleBuffer::CircleBuffer(void)
 {
     index = 0;
@@ -15,24 +13,30 @@
     }
 }
 
+
 void CircleBuffer::queue(float value[3])
 {
+    index++;
+    if (index >= BUFFERSIZE) 
+    {
+        index = 0;
+    }
     for (int i = 0; i < 3 ; i++)
     {
         buffersum[i] -= buffer[index][i];
         buffersum[i] += value[i];
         buffer[index][i] = value[i];
     }
-    index++;
-    if (index >= BUFFERSIZE) 
-    {
-        index = 0;
-    }
+
 }
 
 void CircleBuffer::read(float rawfilter[3])
 {
-    rawfilter[0] = buffersum[0] / BUFFERSIZE;
-    rawfilter[1] = buffersum[1] / BUFFERSIZE;
-    rawfilter[2] = buffersum[2] / BUFFERSIZE;
+//    rawfilter[0] = buffersum[0] / BUFFERSIZE;
+//    rawfilter[1] = buffersum[1] / BUFFERSIZE;
+//    rawfilter[2] = buffersum[2] / BUFFERSIZE;
+    
+    rawfilter[0] = (buffersum[0] + buffer[index][0]) / (BUFFERSIZE + 1);
+    rawfilter[1] = (buffersum[1] + buffer[index][1]) / (BUFFERSIZE + 1);
+    rawfilter[2] = (buffersum[2] + buffer[index][2]) / (BUFFERSIZE + 1);
 }
\ No newline at end of file