diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d222d4f8ec..a77841d3f3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -354,6 +354,12 @@ private: uint32_t last_edge_time_ms; // system time that switch position was last changed } control_switch_state; + // de-bounce counters for switches.cpp + struct debounce { + uint8_t count; + uint8_t ch_flag; + } aux_debounce[(CH_12 - CH_7)+1]; + typedef struct { bool running; float max_speed; @@ -901,6 +907,7 @@ private: void init_aux_switches(); void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag); void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag); + bool debounce_aux_switch(uint8_t chan, uint8_t ch_flag); void save_trim(); void auto_trim(); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 2493ac575e..c33cfe294f 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -125,7 +125,7 @@ uint8_t Copter::read_3pos_switch(uint8_t chan) #define read_aux_switch(chan, flag, option) \ do { \ switch_position = read_3pos_switch(chan); \ - if (flag != switch_position) { \ + if (debounce_aux_switch(chan, flag) && flag != switch_position) { \ flag = switch_position; \ do_aux_switch_function(option, flag); \ } \ @@ -209,6 +209,32 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) } } +/* + debounce an aux switch using a counter in copter.debounce + structure. This will return false until the same ch_flag is seen debounce_count times + */ +bool Copter::debounce_aux_switch(uint8_t chan, uint8_t ch_flag) +{ + // a value of 2 means we need 3 values in a row with the same + // value to activate + const uint8_t debounce_count = 2; + + if (chan < CH_7 || chan > CH_12) { + // someone has forgotten to expand the debounce channel range + return false; + } + struct debounce &db = aux_debounce[chan-CH_7]; + if (db.ch_flag != ch_flag) { + db.ch_flag = ch_flag; + db.count = 0; + return false; + } + if (db.count < debounce_count) { + db.count++; + } + return db.count >= debounce_count; +} + // do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) {