From 301ab00c5b64db54f1a32065602a5bab900a0fd0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 1 Aug 2012 13:59:20 +1000
Subject: [PATCH] APM: added control switch debouncer

this adds a switch debouncer, similar to the one used in
ArduCopter. I'm adding this after a flight on the weekend where noise
on the control mode channel caused a mode change away from auto.

To prevent this change adding excessive mode switch latency, it also
moves the reading of the control switch to the 10Hz loop, away from
the 3.3Hz loop. That gives us 0.2s delay in mode switch changes and
allows for spikes in the control mode for 0.1 seconds without changing
mode.
---
 ArduPlane/ArduPlane.pde     |  7 +++----
 ArduPlane/control_modes.pde | 14 +++++++++++++-
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index d2163c5bc1..66e44e47f5 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -807,6 +807,9 @@ Serial.println(tempaccel.z, DEC);
 		case 1:
 			medium_loopCounter++;
 
+			// Read 6-position switch on radio
+			// -------------------------------
+			read_control_switch();
 
 			if(g_gps->new_data){
 				g_gps->new_data 	= false;
@@ -896,10 +899,6 @@ static void slow_loop()
 		case 1:
 			slow_loopCounter++;
 
-			// Read 3-position switch on radio
-			// -------------------------------
-			read_control_switch();
-
 			// Read Control Surfaces/Mix switches
 			// ----------------------------------
 			update_servo_switches();
diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde
index 1c545559ef..c70a4b2796 100644
--- a/ArduPlane/control_modes.pde
+++ b/ArduPlane/control_modes.pde
@@ -1,8 +1,9 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+
 static void read_control_switch()
 {
-	
+	static bool switch_debouncer;
 	byte switchPosition = readSwitch();
 	
 	// If switchPosition = 255 this indicates that the mode control channel input was out of range
@@ -19,6 +20,15 @@ static void read_control_switch()
         (g.reset_switch_chan != 0 && 
          APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
 
+        if (switch_debouncer == false) {
+            // this ensures that mode switches only happen if the
+            // switch changes for 2 reads. This prevents momentary
+            // spikes in the mode control channel from causing a mode
+            // switch
+            switch_debouncer = true;
+            return;
+        }
+
 		set_mode(flight_modes[switchPosition]);
 
 		oldSwitchPosition = switchPosition;
@@ -29,6 +39,8 @@ static void read_control_switch()
 		reset_I();
 	}
 
+    switch_debouncer = false;
+
     if (g.inverted_flight_ch != 0) {
         // if the user has configured an inverted flight channel, then
         // fly upside down when that channel goes above INVERTED_FLIGHT_PWM