From 9ccf043884fc11ed5c6c628e4f88b39beb431f41 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 10 Oct 2022 11:34:19 +1100
Subject: [PATCH] RC_Channel: added cache of aux functions for scripting

allows for scripting to act on aux functions
---
 libraries/RC_Channel/RC_Channel.cpp  |  3 +++
 libraries/RC_Channel/RC_Channel.h    | 18 +++++++++++++
 libraries/RC_Channel/RC_Channels.cpp | 39 ++++++++++++++++++++++++++++
 3 files changed, 60 insertions(+)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 667e32b10e..522e958b67 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -939,6 +939,9 @@ void RC_Channel::do_aux_function_fft_notch_tune(const AuxSwitchPos ch_flag)
 
 bool RC_Channel::run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source)
 {
+#if AP_SCRIPTING_ENABLED
+    rc().set_aux_cached(ch_option, pos);
+#endif
     const bool ret = do_aux_function(ch_option, pos);
 
     // @LoggerMessage: AUXF
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 36c4df07e9..e257c0a92c 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -5,6 +5,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Common/Bitmask.h>
 
 #define NUM_RC_CHANNELS 16
 
@@ -260,6 +261,9 @@ public:
         SCRIPTING_6 =        305,
         SCRIPTING_7 =        306,
         SCRIPTING_8 =        307,
+
+        // this must be higher than any aux function above
+        AUX_FUNCTION_MAX =   308,
     };
     typedef enum AUX_FUNC aux_func_t;
 
@@ -577,6 +581,11 @@ public:
     void calibrating(bool b) { gcs_is_calibrating = b; }
     bool calibrating() { return gcs_is_calibrating; }
 
+#if AP_SCRIPTING_ENABLED
+    // get last aux cached value for scripting. Returns false if never set, otherwise 0,1,2
+    bool get_aux_cached(RC_Channel::aux_func_t aux_fn, uint8_t &pos);
+#endif
+    
 protected:
 
     enum class Option {
@@ -618,6 +627,15 @@ private:
 
     // true if GCS is performing a RC calibration
     bool gcs_is_calibrating;
+
+#if AP_SCRIPTING_ENABLED
+    // bitmask of last aux function value, 2 bits per function
+    // value 0 means never set, otherwise level+1
+    HAL_Semaphore aux_cache_sem;
+    Bitmask<unsigned(RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX)*2> aux_cached;
+
+    void set_aux_cached(RC_Channel::aux_func_t aux_fn, RC_Channel::AuxSwitchPos pos);
+#endif
 };
 
 RC_Channels &rc();
diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp
index 328a183203..21915176b1 100644
--- a/libraries/RC_Channel/RC_Channels.cpp
+++ b/libraries/RC_Channel/RC_Channels.cpp
@@ -260,6 +260,45 @@ uint32_t RC_Channels::enabled_protocols() const
     return uint32_t(_protocols.get());
 }
 
+#if AP_SCRIPTING_ENABLED
+/*
+  implement aux function cache for scripting
+ */
+
+/*
+  get last aux cached value for scripting. Returns false if never set, otherwise 0,1,2
+*/
+bool RC_Channels::get_aux_cached(RC_Channel::aux_func_t aux_fn, uint8_t &pos)
+{
+    const uint16_t aux_idx = uint16_t(aux_fn);
+    if (aux_idx >= unsigned(RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX)) {
+        return false;
+    }
+    WITH_SEMAPHORE(aux_cache_sem);
+    uint8_t v = aux_cached.get(aux_idx*2) | (aux_cached.get(aux_idx*2+1)<<1);
+    if (v == 0) {
+        // never been set
+        return false;
+    }
+    pos = v-1;
+    return true;
+}
+
+/*
+  set cached value of an aux function
+ */
+void RC_Channels::set_aux_cached(RC_Channel::aux_func_t aux_fn, RC_Channel::AuxSwitchPos pos)
+{
+    const uint16_t aux_idx = uint16_t(aux_fn);
+    if (aux_idx < unsigned(RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX)) {
+        WITH_SEMAPHORE(aux_cache_sem);
+        uint8_t v = unsigned(pos)+1;
+        aux_cached.setonoff(aux_idx*2, v&1);
+        aux_cached.setonoff(aux_idx*2+1, v>>1);
+    }
+}
+#endif // AP_SCRIPTING_ENABLED
+
 // singleton instance
 RC_Channels *RC_Channels::_singleton;