diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py
index aa43925ce5..7f8f3b6e45 100644
--- a/Tools/autotest/apmrover2.py
+++ b/Tools/autotest/apmrover2.py
@@ -328,7 +328,7 @@ class AutoTestRover(AutoTest):
         self.set_rc(3, 1500)
         self.wait_mode('AUTO')
         self.wait_waypoint(1, 4, max_dist=5)
-        self.wait_mode('HOLD')
+        self.wait_mode('HOLD', timeout=300)
         self.progress("Mission OK")
 
     def drive_mission_rover1(self):
@@ -465,6 +465,83 @@ class AutoTestRover(AutoTest):
             raise NotAchievedException()
         self.progress("Pin mask changed after relay command")
 
+    def test_setting_modes_via_mavproxy_switch(self):
+        fnoo = [(1, 'MANUAL'),
+                (2, 'MANUAL'),
+                (3, 'RTL'),
+                #                (4, 'AUTO'), // no mission, can't set auto
+                (5, 'RTL'), # non-existant mode, should stay in RTL
+                (6, 'MANUAL')]
+        for (num, expected) in fnoo:
+            self.mavproxy.send('switch %u\n' % num)
+            self.wait_mode(expected)
+
+    def test_setting_modes_via_modeswitch(self):
+        # test setting of modes through mode switch
+        self.context_push();
+        ex = None
+        try:
+            self.set_parameter("MODE_CH", 8)
+            self.set_rc(8, 1000)
+            # mavutil.mavlink.ROVER_MODE_HOLD:
+            self.set_parameter("MODE6", 4)
+            # mavutil.mavlink.ROVER_MODE_ACRO
+            self.set_parameter("MODE5", 1)
+            self.set_rc(8, 1800) # PWM for mode6
+            self.wait_mode("HOLD")
+            self.set_rc(8, 1700) # PWM for mode5
+            self.wait_mode("ACRO")
+            self.set_rc(8, 1800) # PWM for mode6
+            self.wait_mode("HOLD")
+            self.set_rc(8, 1700) # PWM for mode5
+            self.wait_mode("ACRO")
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.context_pop();
+
+        if ex is not None:
+            raise ex
+
+    def test_setting_modes_via_auxswitches(self):
+        self.context_push();
+        ex = None
+        try:
+            self.set_parameter("MODE5", 1)
+            self.mavproxy.send('switch 5\n')  # acro mode
+            self.wait_mode("ACRO")
+            self.set_rc(9, 1000)
+            self.set_rc(10, 1000)
+            self.set_parameter("RC9_OPTION", 53) # steering
+            self.set_parameter("RC10_OPTION", 54) # hold
+            self.set_rc(9, 1900)
+            self.wait_mode("STEERING")
+            self.set_rc(10, 1900)
+            self.wait_mode("HOLD")
+
+            # reset both switches - should go back to ACRO
+            self.set_rc(9, 1000)
+            self.set_rc(10, 1000)
+            self.wait_mode("ACRO")
+
+            self.set_rc(9, 1900)
+            self.wait_mode("STEERING")
+            self.set_rc(10, 1900)
+            self.wait_mode("HOLD")
+
+            self.set_rc(10, 1000) # this re-polls the mode switch
+            self.wait_mode("ACRO")
+            self.set_rc(9, 1000)
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.context_pop();
+
+        if ex is not None:
+            raise ex
+
     def autotest(self):
         """Autotest APMrover2 in SITL."""
         if not self.hasInit:
@@ -489,6 +566,15 @@ class AutoTestRover(AutoTest):
             self.wait_ready_to_arm()
             self.arm_vehicle()
 
+            self.run_test("Set modes via mavproxy switch",
+                          self.test_setting_modes_via_mavproxy_switch)
+
+            self.run_test("Set modes via modeswitch",
+                          self.test_setting_modes_via_modeswitch)
+
+            self.run_test("Set modes via auxswitches",
+                          self.test_setting_modes_via_auxswitches)
+
             self.run_test("Drive an RTL Mission", self.drive_rtl_mission)
 
             self.run_test("Learn/Drive Square with Ch7 option",
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 1927ef8c35..6cb9d73798 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -633,7 +633,7 @@ class AutoTestCopter(AutoTest):
         self.set_rc(3, 1800)
 
         # wait for fence to trigger
-        self.wait_mode('RTL')
+        self.wait_mode('RTL', timeout=120)
 
         self.progress("Waiting for disarm")
         self.mav.motors_disarmed_wait()
@@ -1297,6 +1297,79 @@ class AutoTestCopter(AutoTest):
         if ex is not None:
             raise ex
 
+    def test_setting_modes_via_modeswitch(self):
+        self.context_push();
+        ex = None
+        try:
+            fltmode_ch = 5
+            self.set_parameter("FLTMODE_CH", fltmode_ch)
+            self.set_rc(fltmode_ch, 1000) # PWM for mode1
+            testmodes = [("FLTMODE1", 4, "GUIDED", 1165),
+                         ("FLTMODE2", 13, "SPORT", 1295),
+                         ("FLTMODE3", 6, "RTL", 1425),
+                         ("FLTMODE4", 7, "CIRCLE", 1555),
+                         ("FLTMODE5", 1, "ACRO", 1685),
+                         ("FLTMODE6", 17, "BRAKE", 1815),
+            ]
+            for mode in testmodes:
+                (parm, parm_value, name, pwm) = mode
+                self.set_parameter(parm, parm_value)
+
+            for mode in reversed(testmodes):
+                (parm, parm_value, name, pwm) = mode
+                self.set_rc(fltmode_ch, pwm)
+                self.wait_mode(name)
+
+            for mode in testmodes:
+                (parm, parm_value, name, pwm) = mode
+                self.set_rc(fltmode_ch, pwm)
+                self.wait_mode(name)
+
+            for mode in reversed(testmodes):
+                (parm, parm_value, name, pwm) = mode
+                self.set_rc(fltmode_ch, pwm)
+                self.wait_mode(name)
+
+            self.mavproxy.send('switch 6\n')
+            self.wait_mode("BRAKE")
+            self.mavproxy.send('switch 5\n')
+            self.wait_mode("ACRO")
+
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.context_pop()
+
+        if ex is not None:
+            raise ex
+
+    def test_setting_modes_via_auxswitch(self):
+        self.context_push();
+        ex = None
+        try:
+            fltmode_ch = int(self.get_parameter("FLTMODE_CH"))
+            self.set_rc(fltmode_ch, 1000)
+            self.wait_mode("CIRCLE")
+            self.set_rc(9, 1000)
+            self.set_rc(10, 1000)
+            self.set_parameter("RC9_OPTION", 18) # land
+            self.set_parameter("RC10_OPTION", 55) # guided
+            self.set_rc(9, 1900)
+            self.wait_mode("LAND")
+            self.set_rc(10, 1900)
+            self.wait_mode("GUIDED")
+            self.set_rc(10, 1000) # this re-polls the mode switch
+            self.wait_mode("CIRCLE")
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.context_pop();
+
+        if ex is not None:
+            raise ex
+
     def guided_achieve_heading(self, heading):
         tstart = self.get_sim_time()
         self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW,
@@ -1415,6 +1488,15 @@ class AutoTestCopter(AutoTest):
             self.progress("Waiting reading for arm")
             self.wait_ready_to_arm()
 
+            self.run_test("Set modes via modeswitch",
+                          self.test_setting_modes_via_modeswitch)
+
+            self.run_test("Set modes via auxswitch",
+                          self.test_setting_modes_via_auxswitch)
+
+            self.mavproxy.send('switch 6\n')  # stabilize mode
+            self.wait_mode('STABILIZE')
+
             # Arm
             self.run_test("Arm motors", self.arm_vehicle)
 
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 79db695eb7..e14d7c85f6 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -223,6 +223,11 @@ class AutoTest(ABC):
                         self.buildlogs_path("%s-valgrind.log" %
                                             self.log_name))
 
+    def start_test(self, description):
+        self.progress("#")
+        self.progress("########## %s  ##########" % description)
+        self.progress("#")
+
     #################################################
     # GENERAL UTILITIES
     #################################################
@@ -729,7 +734,7 @@ class AutoTest(ABC):
                       (wpnum_end, wpnum_end))
         raise WaitWaypointTimeout()
 
-    def wait_mode(self, mode, timeout=None):
+    def wait_mode(self, mode, timeout=60):
         """Wait for mode to change."""
         mode_map = self.mav.mode_mapping()
         if mode_map is None or mode not in mode_map:
@@ -738,15 +743,15 @@ class AutoTest(ABC):
             raise ErrorException()
         self.progress("Waiting for mode %s" % mode)
         tstart = self.get_sim_time()
-        hastimeout = False
-        while self.mav.flightmode != mode and not hastimeout:
-            if timeout is not None:
-                hastimeout = self.get_sim_time() > tstart + timeout
+        self.mav.wait_heartbeat()
+        while self.mav.flightmode != mode:
+            if (timeout is not None and
+                self.get_sim_time() > tstart + timeout):
+                raise WaitModeTimeout()
             self.mav.wait_heartbeat()
+#            self.progress("heartbeat mode %s Want: %s" % (
+#                    self.mav.flightmode, mode))
         self.progress("Got mode %s" % mode)
-        if self.mav.flightmode != mode and hastimeout:
-            raise WaitModeTimeout()
-        return True
 
     def wait_ready_to_arm(self, timeout=None):
         # wait for EKF checks to pass
@@ -791,7 +796,7 @@ class AutoTest(ABC):
         self.progress("Failed to get EKF.flags=%u" % required_value)
         raise AutoTestTimeoutException()
 
-    def run_test(self, desc, function):
+    def run_test(self, desc, function, interact=False):
         self.progress("#")
         self.progress("########## %s ##########" % (desc))
         self.progress("#")
@@ -801,6 +806,9 @@ class AutoTest(ABC):
         except Exception as e:
             self.progress('FAILED: "%s": %s' % (desc, repr(e)))
             self.fail_list.append((desc, e))
+            if interact:
+                self.progress("Starting MAVProxy interaction as directed")
+                self.mavproxy.interact()
             return
         self.progress('PASSED: "%s"' % desc)
 
diff --git a/Tools/autotest/default_params/copter.parm b/Tools/autotest/default_params/copter.parm
index 3e4e03d15b..6c0cd46842 100644
--- a/Tools/autotest/default_params/copter.parm
+++ b/Tools/autotest/default_params/copter.parm
@@ -3,7 +3,6 @@ FRAME_TYPE	0
 MAG_ENABLE	1
 FS_THR_ENABLE   1
 BATT_MONITOR    4
-CH7_OPT         7
 COMPASS_LEARN   0
 COMPASS_OFS_X   5
 COMPASS_OFS_Y   13
@@ -33,6 +32,7 @@ RC6_MIN         1000.000000
 RC6_TRIM        1500.000000
 RC7_MAX         2000.000000
 RC7_MIN         1000.000000
+RC7_OPTION      7
 RC7_TRIM        1500.000000
 RC8_MAX         2000.000000
 RC8_MIN         1000.000000
diff --git a/Tools/scripts/runfliptest.py b/Tools/scripts/runfliptest.py
index 1aa638307a..02747ee002 100755
--- a/Tools/scripts/runfliptest.py
+++ b/Tools/scripts/runfliptest.py
@@ -47,7 +47,7 @@ mavproxy.expect('ARMED')
 mavproxy.send('alt_hold\n')
 wait_mode(mav, ['ALT_HOLD'])
 mavproxy.send('rc 3 1800\n')
-mavproxy.send('param set CH7_OPT 2\n')
+mavproxy.send('param set RC7_OPTION 2\n')
 mavproxy.send('repeat add 2 rc 7 2000\n')
 wait_time(mav, 1)
 mavproxy.send('repeat add 2 rc 7 1000\n')