robobuggy / arduino / RadioBuggyMega / filter.c @ 6b9a71b0
History | View | Annotate | Download (821 Bytes)
1 |
#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 |
|
38 |
|