diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index d832d8fed3..ba93f53037 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -63,7 +63,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(check_short_failsafe,   50,    100,   9),
     SCHED_TASK(update_speed_height,    50,    200,  12),
     SCHED_TASK(update_throttle_hover, 100,     90,  24),
-    SCHED_TASK(read_control_switch,     7,    100,  27),
+    SCHED_TASK_CLASS(RC_Channels,     (RC_Channels*)&plane.g2.rc_channels, read_mode_switch,           7,    100, 27),
     SCHED_TASK(update_GPS_50Hz,        50,    300,  30),
     SCHED_TASK(update_GPS_10Hz,        10,    400,  33),
     SCHED_TASK(navigate,               10,    150,  36),
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index ee58db454f..89ed040993 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -190,6 +190,7 @@ private:
 
     // flight modes convenience array
     AP_Int8 *flight_modes = &g.flight_mode1;
+    const uint8_t num_flight_modes = 6;
 
     AP_FixedWing::Rangefinder_State rangefinder_state;
 
@@ -966,7 +967,6 @@ private:
     // control_modes.cpp
     void read_control_switch();
     uint8_t readSwitch(void) const;
-    void reset_control_switch();
     void autotune_start(void);
     void autotune_restore(void);
     void autotune_enable(bool enable);
diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
index 1da0108f6c..88c24cb4e4 100644
--- a/ArduPlane/RC_Channel.cpp
+++ b/ArduPlane/RC_Channel.cpp
@@ -45,8 +45,7 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
         // return to flight mode switch's flight mode if we are currently
         // in this mode
         if (plane.control_mode->mode_number() == number) {
-// TODO:           rc().reset_mode_switch();
-            plane.reset_control_switch();
+            rc().reset_mode_switch();
         }
     }
 }
@@ -355,7 +354,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         break;
 
     case AUX_FUNC::MODE_SWITCH_RESET:
-        plane.reset_control_switch();
+        rc().reset_mode_switch();
         break;
 
     case AUX_FUNC::CRUISE:
diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h
index 35663554a5..a65f8bb335 100644
--- a/ArduPlane/RC_Channel.h
+++ b/ArduPlane/RC_Channel.h
@@ -13,6 +13,9 @@ protected:
                            AuxSwitchPos ch_flag) override;
     bool do_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
 
+    // called when the mode switch changes position:
+    void mode_switch_changed(modeswitch_pos_t new_pos) override;
+
 private:
 
     void do_aux_function_change_mode(Mode::Number number,
@@ -46,6 +49,8 @@ public:
 
     RC_Channel *get_arming_channel(void) const override;
 
+    void read_mode_switch() override;
+
 protected:
 
     // note that these callbacks are not presently used on Plane:
diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp
index 8caeae8204..7f15e0e266 100644
--- a/ArduPlane/control_modes.cpp
+++ b/ArduPlane/control_modes.cpp
@@ -98,61 +98,23 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
     return ret;
 }
 
-void Plane::read_control_switch()
+void RC_Channels_Plane::read_mode_switch()
 {
-    static bool switch_debouncer;
-    uint8_t switchPosition = readSwitch();
-
-    // If switchPosition = 255 this indicates that the mode control channel input was out of range
-    // If we get this value we do not want to change modes.
-    if(switchPosition == 255) return;
-
-    if (!rc().has_valid_input()) {
-        // ignore the mode switch channel if there is no valid RC input
-        return;
-    }
-
-    if (millis() - failsafe.last_valid_rc_ms > 100) {
+    if (millis() - plane.failsafe.last_valid_rc_ms > 100) {
         // only use signals that are less than 0.1s old.
         return;
     }
+    RC_Channels::read_mode_switch();
+}
 
-    if (oldSwitchPosition != switchPosition) {
-
-        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_by_number((enum Mode::Number)flight_modes[switchPosition].get(), ModeReason::RC_COMMAND);
-
-        oldSwitchPosition = switchPosition;
+void RC_Channel_Plane::mode_switch_changed(modeswitch_pos_t new_pos)
+{
+    if (new_pos < 0 || (uint8_t)new_pos > plane.num_flight_modes) {
+        // should not have been called
+        return;
     }
 
-    switch_debouncer = false;
-
-}
-
-uint8_t Plane::readSwitch(void) const
-{
-    uint16_t pulsewidth = RC_Channels::get_radio_in(g.flight_mode_channel - 1);
-    if (pulsewidth <= 900 || pulsewidth >= 2200) return 255;            // This is an error condition
-    if (pulsewidth <= 1230) return 0;
-    if (pulsewidth <= 1360) return 1;
-    if (pulsewidth <= 1490) return 2;
-    if (pulsewidth <= 1620) return 3;
-    if (pulsewidth <= 1749) return 4;              // Software Manual
-    return 5;                                                           // Hardware Manual
-}
-
-void Plane::reset_control_switch()
-{
-    oldSwitchPosition = 254;
-    read_control_switch();
+    plane.set_mode_by_number((Mode::Number)plane.flight_modes[new_pos].get(), ModeReason::RC_COMMAND);
 }
 
 /*
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 149aa273e3..e0ae140fd3 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -142,7 +142,7 @@ void Plane::init_ardupilot()
 
     // set the correct flight mode
     // ---------------------------
-    reset_control_switch();
+    rc().reset_mode_switch();
 
     // initialise sensor
 #if AP_OPTICALFLOW_ENABLED