WeightedAverage.h

00001 #ifndef WEIGHTED_AVERAGE_H
00002 #define WEIGHTED_AVERAGE_H
00003 
00004 // Updated every 4th sample, ~16ms
00005 #define WEIGHTED_BUFFER_SIZE 64
00006 
00007 class WeightedAverage
00008 {
00009     public: 
00010         explicit WeightedAverage()
00011         {
00012             reset();
00013         }
00014         
00015         void add(float reading)
00016         {
00017             total -= buff[ptr % BUFFER_SIZE];
00018             total += reading;
00019             buff[ptr++] = reading;
00020         }
00021         
00022         float average()
00023         {
00024             float avg = 0;
00025             
00026             int n = (BUFFER_SIZE > ptr ? BUFFER_SIZE : ptr);
00027             
00028             for(int i = 0; i < n; i++)
00029             {
00030                 avg += (i * buff[i]);   
00031             }
00032             
00033             return avg / ((n * (n + 1)) / 2);
00034         }
00035         
00036         void reset()
00037         {
00038             ptr = 0;
00039             total = 0;
00040             for(int i = 0; i < BUFFER_SIZE; i++)
00041                 buff[i] = 0.0f;
00042         }
00043         
00044     private:
00045         unsigned long ptr;
00046         float total;
00047         float buff [WEIGHTED_BUFFER_SIZE];
00048 };
00049 
00050 WeightedAverage leftWeightedAvg;
00051 WeightedAverage rightWeightedAvg;
00052 WeightedAverage frontWeightedAvg;
00053 
00054 #endif