File content as of revision 1:8c7e5801d763:

#include "CircleBuffer.h"

CircleBuffer::CircleBuffer(void)
{
    index = 0;
    for (int i = 0; i < 3; i++)
    {
        buffersum[i] = 0;
        for (int j = 0; j < BUFFERSIZE; j++) 
        {
            buffer[j][i] = 0;
        }
    }
}


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];
    }

}

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] + buffer[index][0]) / (BUFFERSIZE + 1);
    rawfilter[1] = (buffersum[1] + buffer[index][1]) / (BUFFERSIZE + 1);
    rawfilter[2] = (buffersum[2] + buffer[index][2]) / (BUFFERSIZE + 1);
}