diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 7d86f082e5..af1a5d8b85 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/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 diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 9192272003..d7aa45882c 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -5,6 +5,7 @@ #include #include #include +#include #ifndef AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED #define AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED 1 @@ -268,6 +269,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; @@ -592,6 +596,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 { @@ -635,6 +644,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 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;