Revision 0c80370b
Added struct filter_state, so we can filter multiple signals
| arduino/RadioBuggyMega/RadioBuggyMega.ino | ||
|---|---|---|
| 32 | 32 |
static char data; // used to pass things into xbee |
| 33 | 33 |
static unsigned long last_time; |
| 34 | 34 |
|
| 35 |
// Initialize filter for |
|
| 36 |
struct filter_state ail_state; |
|
| 37 |
struct filter_state thr_state; |
|
| 35 | 38 |
|
| 36 | 39 |
#define LED_DANGER_PIN 12 |
| 37 | 40 |
|
| ... | ... | |
| 45 | 48 |
digitalWrite(LED_DANGER_PIN, HIGH); |
| 46 | 49 |
} |
| 47 | 50 |
|
| 51 |
|
|
| 52 |
|
|
| 48 | 53 |
void setup() {
|
| 49 | 54 |
Serial.begin(9600); |
| 50 | 55 |
Serial1.begin(9600); |
| ... | ... | |
| 54 | 59 |
// Initialize Buggy |
| 55 | 60 |
// Pins 2 and 3: pin 2 is thr, pin 3 is ail |
| 56 | 61 |
receiver_init(); |
| 62 |
filter_init(&ail_state); |
|
| 63 |
filter_init(&thr_state); |
|
| 57 | 64 |
watchdog_init(TIME_THRESH, &watchdog_fail); |
| 58 | 65 |
brake_init(BRAKE_PIN, BRAKE_INDICATOR_PIN); |
| 59 | 66 |
steering_init(STEERING_PIN, 120, 133, 145); |
| ... | ... | |
| 85 | 92 |
|
| 86 | 93 |
void loop() {
|
| 87 | 94 |
|
| 88 |
if(filter(rc_available[THR_INDEX])) {
|
|
| 95 |
if(filter_loop(&thr_state, rc_available[THR_INDEX])) {
|
|
| 89 | 96 |
watchdog_feed(); |
| 90 | 97 |
rc_angle = receiver_get_angle(THR_INDEX); |
| 91 | 98 |
steer_angle = convert_rc_to_steering(rc_angle); |
| 92 | 99 |
steering_set(steer_angle); |
| 93 | 100 |
} |
| 94 | 101 |
|
| 95 |
if(filter(rc_available[AIL_INDEX])) {
|
|
| 102 |
if(filter_loop(&ail_state, rc_available[AIL_INDEX])) {
|
|
| 96 | 103 |
watchdog_feed(); |
| 97 | 104 |
rc_thr = receiver_get_angle(AIL_INDEX); |
| 98 | 105 |
// TODO make this code...less...something |
| 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 |
} |
|
| arduino/RadioBuggyMega/filter.h | ||
|---|---|---|
| 13 | 13 |
extern "C"{
|
| 14 | 14 |
#endif |
| 15 | 15 |
|
| 16 |
#define FILTER_LEN 5 // Averaging window size |
|
| 16 | 17 |
|
| 17 |
int filter(int val); // returns the int that has been filtered |
|
| 18 |
struct filter_state {
|
|
| 19 |
int prev_values[FILTER_LEN]; |
|
| 20 |
char started; |
|
| 21 |
int pos; // next index to update |
|
| 22 |
}; |
|
| 23 |
|
|
| 24 |
|
|
| 25 |
void filter_init(struct filter_state *state); |
|
| 26 |
|
|
| 27 |
int filter_loop(struct filter_state *state, int val); // returns the int that has been filtered |
|
| 18 | 28 |
|
| 19 | 29 |
|
| 20 | 30 |
#ifdef __cplusplus |
| ... | ... | |
| 23 | 33 |
|
| 24 | 34 |
#endif |
| 25 | 35 |
|
| 26 |
|
|
| 36 |
|
|
Also available in: Unified diff