robobuggy / arduino / RadioBuggyMega / filter.c @ 0c80370b
History | View | Annotate | Download (1005 Bytes)
1 |
#include <Arduino.h> |
---|---|
2 |
#include "receiver.h" |
3 |
#include "filter.h" |
4 |
|
5 |
// This only does preliminary initialization.
|
6 |
// More initialization is done in the function body.
|
7 |
void filter_init(struct filter_state *state) { |
8 |
state->started = 0;
|
9 |
} |
10 |
|
11 |
int filter_loop(struct filter_state *state, int val){ |
12 |
int i;
|
13 |
// First initialization: on assumption val is never -1
|
14 |
if(!(state->started)){
|
15 |
for(i = 0; i < FILTER_LEN; i++){ |
16 |
state->prev_values[i] = val; |
17 |
} |
18 |
state->pos = 0; // initialize the positions of the next number to be changed |
19 |
state->started = 1;
|
20 |
return val;
|
21 |
} |
22 |
|
23 |
// NOT first initialization.
|
24 |
state->prev_values[state->pos] = val; // replace the number in the array
|
25 |
state->pos = (state->pos+1)%FILTER_LEN;// increment the pos number |
26 |
// calculate average of array
|
27 |
int sum = 0; |
28 |
for(i = 0; i < FILTER_LEN; i++){ |
29 |
sum += state->prev_values[i]; |
30 |
} |
31 |
int avg = sum/FILTER_LEN;
|
32 |
// return.
|
33 |
return avg;
|
34 |
|
35 |
} |