robobuggy / arduino / RadioBuggyMega / filter.c @ 6b9a71b0
History | View | Annotate | Download (821 Bytes)
1 | 6b9a71b0 | unknown | #include <Arduino.h> |
---|---|---|---|
2 | #include "receiver.h" |
||
3 | #include "filter.h" |
||
4 | |||
5 | |||
6 | #define SIZE 5 // the array size |
||
7 | static char started = 0; |
||
8 | static int last_val_sent; |
||
9 | static int values[5]; |
||
10 | static int pos; |
||
11 | |||
12 | int filter(int val){ |
||
13 | int i;
|
||
14 | // First initialization: on assumption val is never -1
|
||
15 | if(!started){
|
||
16 | for(i = 0; i < SIZE; i++){ |
||
17 | values[i] = val; |
||
18 | } |
||
19 | pos = 0; // initialize the positions of the next number to be changed |
||
20 | started = 1;
|
||
21 | return val;
|
||
22 | } |
||
23 | |||
24 | // NOT first initialization.
|
||
25 | values[pos] = val; // replace the number in the array
|
||
26 | pos = (pos+1)%SIZE; // increment the pos number |
||
27 | // calculate average of array
|
||
28 | int sum = 0; |
||
29 | for(i = 0; i < SIZE; i++){ |
||
30 | sum += values[i]; |
||
31 | } |
||
32 | int avg = sum/SIZE;
|
||
33 | // return.
|
||
34 | return avg;
|
||
35 | |||
36 | } |
||
37 |