From 584404a6dace6ee97cd918358856153ee553d6c4 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 20 Mar 2019 08:09:56 +1100
Subject: [PATCH] Plane: add RC options for changing mode

---
 ArduPlane/RC_Channel.cpp | 49 +++++++++++++++++++++++++++++++++++++++-
 ArduPlane/RC_Channel.h   |  3 ++-
 2 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
index 7bb7d1c563..4bf6e7cb2a 100644
--- a/ArduPlane/RC_Channel.cpp
+++ b/ArduPlane/RC_Channel.cpp
@@ -27,6 +27,32 @@ bool RC_Channels_Plane::has_valid_input() const
     return true;
 }
 
+void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
+                                                   const aux_switch_pos_t ch_flag)
+{
+    switch(ch_flag) {
+    case HIGH: {
+        // engage mode (if not possible we remain in current flight mode)
+        const bool success = plane.set_mode_by_number(number, MODE_REASON_TX_COMMAND);
+        if (plane.control_mode != &plane.mode_initializing) {
+            if (success) {
+                AP_Notify::events.user_mode_change = 1;
+            } else {
+                AP_Notify::events.user_mode_change_failed = 1;
+            }
+        }
+        break;
+    }
+    default:
+        // 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();
+        }
+    }
+}
+
 void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
                                          const RC_Channel::aux_switch_pos_t ch_flag)
 {
@@ -34,6 +60,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
     // the following functions do not need to be initialised:
     case AUX_FUNC::ARMDISARM:
     case AUX_FUNC::INVERTED:
+    case AUX_FUNC::RTL:
         break;
 
     case AUX_FUNC::REVERSE_THROTTLE:
@@ -66,7 +93,27 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const aux_swi
         plane.reversed_throttle = (ch_flag == HIGH);
         gcs().send_text(MAV_SEVERITY_INFO, "RevThrottle: %s", plane.reversed_throttle?"ENABLE":"DISABLE");
         break;
-        
+
+    case AUX_FUNC::AUTO:
+        do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
+        break;
+
+    case AUX_FUNC::CIRCLE:
+        do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
+        break;
+
+    case AUX_FUNC::GUIDED:
+        do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag);
+        break;
+
+    case AUX_FUNC::MANUAL:
+        do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag);
+        break;
+
+    case AUX_FUNC::RTL:
+        do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
+        break;
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;
diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h
index 5b31e90b00..8c9fd9b507 100644
--- a/ArduPlane/RC_Channel.h
+++ b/ArduPlane/RC_Channel.h
@@ -13,9 +13,10 @@ protected:
                            aux_switch_pos_t ch_flag) override;
     void do_aux_function(aux_func_t ch_option, aux_switch_pos_t) override;
 
-
 private:
 
+    void do_aux_function_change_mode(Mode::Number number,
+                                     aux_switch_pos_t ch_flag);
 };
 
 class RC_Channels_Plane : public RC_Channels