AP_Scripting: added get_aux_cached()

libraries/AP_Common/Bitmask.h

@@ -81,6 +81,15 @@ class Bitmask {
         bits[word] &= ~(1U << ofs);
     }
 
+    // set given bitnumber to on/off
+    void setonoff(uint16_t bit, bool onoff) {
+        if (onoff) {
+            set(bit);
+        } else {
+            clear(bit);
+        }
+    }
+
     // clear all bits
     void clearall(void) {
         memset(bits, 0, numwords*sizeof(bits[0]));

libraries/AP_Relay/AP_Relay.h

@@ -31,6 +31,11 @@ class AP_Relay {
     // de-activate the relay
     void        off(uint8_t instance) { set(instance, false); }
 
+    // get state of relay
+    uint8_t     get(uint8_t instance) const {
+        return instance < AP_RELAY_NUM_RELAYS ? _pin_states & (1U<<instance) : 0;
+    }
+
     // see if the relay is enabled
     bool        enabled(uint8_t instance) { return instance < AP_RELAY_NUM_RELAYS && _pin[instance] != -1; }
 

libraries/AP_Scripting/docs/docs.lua

@@ -1421,6 +1421,11 @@ function rc:get_channel(chan_num) end
 ---@return boolean
 function rc:has_valid_input() end
 
+-- return cached level of aux function
+---@param aux_fn integer
+---@return integer|nil
+function rc:get_aux_cached(aux_fn) end
+
 -- desc
 ---@param aux_fun integer
 ---@param ch_flag integer
@@ -1774,6 +1779,11 @@ function relay:toggle(instance) end
 ---@return boolean
 function relay:enabled(instance) end
 
+-- return state of a relay
+---@param instance integer
+---@return uint8_t
+function relay:get(instance) end
+
 -- desc
 ---@param instance integer
 function relay:off(instance) end

libraries/AP_Scripting/examples/aux_cached.lua

@@ -0,0 +1,37 @@
+--[[ 
+   example for getting cached aux function value
+--]]
+
+local RATE_HZ = 10
+
+local MAV_SEVERITY_ERROR = 3
+local MAV_SEVERITY_INFO = 6
+
+local AUX_FUNCTION_NUM = 302
+
+local last_func_val = nil
+local last_aux_pos = nil
+
+function update()
+   local aux_pos = rc:get_aux_cached(AUX_FUNCTION_NUM)
+   if aux_pos ~= last_aux_pos then
+      last_aux_pos = aux_pos
+      gcs:send_text(MAV_SEVERITY_INFO, string.format("Aux set to %u", aux_pos))
+   end
+end
+
+-- wrapper around update(). This calls update() and if update faults
+-- then an error is displayed, but the script is not stopped
+function protected_wrapper()
+    local success, err = pcall(update)
+    if not success then
+        gcs:send_text(MAV_SEVERITY_ERROR, "Internal Error: " .. err)
+        -- when we fault we run the update function again after 1s, slowing it
+        -- down a bit so we don't flood the console with errors
+        return protected_wrapper, 1000
+    end
+    return protected_wrapper, math.floor(1000 / RATE_HZ)
+end
+
+-- start running update loop
+return protected_wrapper()

libraries/AP_Scripting/generator/description/bindings.desc

@@ -209,6 +209,7 @@ singleton AP_Relay method on void uint8_t 0 AP_RELAY_NUM_RELAYS
 singleton AP_Relay method off void uint8_t 0 AP_RELAY_NUM_RELAYS
 singleton AP_Relay method enabled boolean uint8_t 0 AP_RELAY_NUM_RELAYS
 singleton AP_Relay method toggle void uint8_t 0 AP_RELAY_NUM_RELAYS
+singleton AP_Relay method get uint8_t uint8_t 0 AP_RELAY_NUM_RELAYS
 
 include GCS_MAVLink/GCS.h
 singleton GCS rename gcs
@@ -294,6 +295,7 @@ singleton RC_Channels method run_aux_function boolean RC_Channel::AUX_FUNC'enum
 singleton RC_Channels method has_valid_input boolean
 singleton RC_Channels method lua_rc_channel RC_Channel uint8_t 1 NUM_RC_CHANNELS
 singleton RC_Channels method lua_rc_channel rename get_channel
+singleton RC_Channels method get_aux_cached boolean RC_Channel::AUX_FUNC'enum 0 UINT16_MAX uint8_t'Null
 
 include AP_SerialManager/AP_SerialManager.h
 

libraries/RC_Channel/RC_Channel.cpp

@@ -1017,6 +1017,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

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
 
@@ -264,6 +265,9 @@ class RC_Channel {
         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;
 
@@ -588,6 +592,11 @@ class RC_Channels {
     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 {
@@ -631,6 +640,15 @@ class RC_Channels {
 
     // 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();

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;