From 040f78f51538bef25f1ab517dd4633ef3606db67 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 3 Jun 2020 11:42:20 +1000
Subject: [PATCH] RC_Channel: announce RC switch changes via statustext

---
 libraries/RC_Channel/RC_Channel.cpp | 70 +++++++++++++++++++++++++++++
 libraries/RC_Channel/RC_Channel.h   | 11 +++++
 2 files changed, 81 insertions(+)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 3b212a15e7..3899f972be 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -495,6 +495,56 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
     }
 }
 
+#if !HAL_MINIMIZE_FEATURES
+
+const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
+    { AUX_FUNC::SAVE_WP,"SaveWaypoint"},
+    { AUX_FUNC::CAMERA_TRIGGER,"CameraTrigger"},
+    { AUX_FUNC::RANGEFINDER,"Rangefinder"},
+    { AUX_FUNC::FENCE,"Fence"},
+    { AUX_FUNC::SPRAYER,"Sprayer"},
+    { AUX_FUNC::PARACHUTE_ENABLE,"ParachuteEnable"},
+    { AUX_FUNC::PARACHUTE_RELEASE,"ParachuteRelease"},
+    { AUX_FUNC::PARACHUTE_3POS,"Parachute3Position"},
+    { AUX_FUNC::MISSION_RESET,"MissionReset"},
+    { AUX_FUNC::RETRACT_MOUNT,"RetractMount"},
+    { AUX_FUNC::RELAY,"Relay1"},
+    { AUX_FUNC::LANDING_GEAR,"Landing"},
+    { AUX_FUNC::MOTOR_INTERLOCK,"MotorInterlock"},
+    { AUX_FUNC::RELAY2,"Relay2"},
+    { AUX_FUNC::RELAY3,"Relay3"},
+    { AUX_FUNC::RELAY4,"Relay4"},
+    { AUX_FUNC::PRECISION_LOITER,"PrecisionLoiter"},
+    { AUX_FUNC::AVOID_PROXIMITY,"AvoidPRoximity"},
+    { AUX_FUNC::WINCH_ENABLE,"WinchEnable"},
+    { AUX_FUNC::WINCH_CONTROL,"WinchControl"},
+    { AUX_FUNC::CLEAR_WP,"ClearWaypoint"},
+    { AUX_FUNC::COMPASS_LEARN,"CompassLearn"},
+    { AUX_FUNC::SAILBOAT_TACK,"SailboatTack"},
+    { AUX_FUNC::GPS_DISABLE,"GPSDisable"},
+    { AUX_FUNC::RELAY5,"Relay5"},
+    { AUX_FUNC::RELAY6,"Relay6"},
+    { AUX_FUNC::SAILBOAT_MOTOR_3POS,"SailboatMotor"},
+    { AUX_FUNC::SURFACE_TRACKING,"SurfaceTracking"},
+    { AUX_FUNC::RUNCAM_CONTROL,"RunCamControl"},
+    { AUX_FUNC::RUNCAM_OSD_CONTROL,"RunCamOSDControl"},
+    { AUX_FUNC::VISODOM_CALIBRATE,"VisodomCalibrate"},
+    { AUX_FUNC::CAM_MODE_TOGGLE,"CamModeToggle"},
+};
+
+/* lookup the announcement for switch change */
+const char *RC_Channel::string_for_aux_function(AUX_FUNC function) const     
+{
+     for (const struct LookupTable entry : lookuptable) {
+        if (entry.option == function) {
+            return entry.announcement;
+        }
+     }
+     return nullptr;
+}
+
+#endif // HAL_MINIMIZE_FEATURES
+
 /*
   read an aux channel. Return true if a switch has changed
  */
@@ -515,6 +565,26 @@ bool RC_Channel::read_aux()
         return false;
     }
 
+#if !HAL_MINIMIZE_FEATURES
+    // announce the change to the GCS:
+    const char *aux_string = string_for_aux_function(_option);
+    if (aux_string != nullptr) {
+        const char *temp =  nullptr;
+        switch (new_position) {
+        case AuxSwitchPos::HIGH:
+            temp = "HIGH";           
+            break;
+        case AuxSwitchPos::MIDDLE:
+            temp = "MIDDLE";
+            break;
+        case AuxSwitchPos::LOW:
+            temp = "LOW";          
+            break;
+        }
+        gcs().send_text(MAV_SEVERITY_INFO, "%s %s", aux_string, temp);
+    }
+#endif
+
     // debounced; undertake the action:
     do_aux_function(_option, new_position);
     return true;
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 200bec0383..0b54b30da8 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -274,6 +274,17 @@ private:
     void reset_mode_switch();
     void read_mode_switch();
     bool debounce_completed(int8_t position);
+
+#if !HAL_MINIMIZE_FEATURES
+    // Structure to lookup switch change announcements
+    struct LookupTable{
+       AUX_FUNC option;
+       const char *announcement;
+    };
+
+    static const LookupTable lookuptable[];
+    const char *string_for_aux_function(AUX_FUNC function) const;
+#endif
 };