Revision 0c80370b
Added struct filter_state, so we can filter multiple signals
arduino/RadioBuggyMega/filter.c | ||
---|---|---|
2 | 2 |
#include "receiver.h" |
3 | 3 |
#include "filter.h" |
4 | 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 |
} |
|
5 | 10 |
|
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){ |
|
11 |
int filter_loop(struct filter_state *state, int val){ |
|
13 | 12 |
int i; |
14 | 13 |
// First initialization: on assumption val is never -1 |
15 |
if(!started){
|
|
16 |
for(i = 0; i < SIZE; i++){
|
|
17 |
values[i] = val; |
|
14 |
if(!(state->started)){
|
|
15 |
for(i = 0; i < FILTER_LEN; i++){
|
|
16 |
state->prev_values[i] = val;
|
|
18 | 17 |
} |
19 |
pos = 0; // initialize the positions of the next number to be changed |
|
20 |
started = 1; |
|
18 |
state->pos = 0; // initialize the positions of the next number to be changed
|
|
19 |
state->started = 1;
|
|
21 | 20 |
return val; |
22 | 21 |
} |
23 | 22 |
|
24 | 23 |
// NOT first initialization. |
25 |
values[pos] = val; // replace the number in the array
|
|
26 |
pos = (pos+1)%SIZE; // increment the pos number
|
|
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
|
|
27 | 26 |
// calculate average of array |
28 | 27 |
int sum = 0; |
29 |
for(i = 0; i < SIZE; i++){
|
|
30 |
sum += values[i]; |
|
28 |
for(i = 0; i < FILTER_LEN; i++){
|
|
29 |
sum += state->prev_values[i];
|
|
31 | 30 |
} |
32 |
int avg = sum/SIZE;
|
|
31 |
int avg = sum/FILTER_LEN;
|
|
33 | 32 |
// return. |
34 | 33 |
return avg; |
35 | 34 |
|
36 |
} |
|
37 |
|
|
38 |
|
|
35 |
} |
Also available in: Unified diff