iNavFlight · DzikuVx · Apr 26, 2020
diff --git a/src/main/common/global_functions.c b/src/main/common/global_functions.c
@@ -33,16 +33,18 @@
 
 #include "io/vtx.h"
 #include "drivers/vtx_common.h"
+#include "rx/rx.h"
 
 #ifdef USE_GLOBAL_FUNCTIONS
 
 #include "common/axis.h"
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
+PG_REGISTER_ARRAY_WITH_RESET_FN(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 1);
 
 EXTENDED_FASTRAM uint64_t globalFunctionsFlags = 0;
 EXTENDED_FASTRAM globalFunctionState_t globalFunctionsStates[MAX_GLOBAL_FUNCTIONS];
 EXTENDED_FASTRAM int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST];
+EXTENDED_FASTRAM rcChannelOverride_t channelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
 void pgResetFn_globalFunctions(globalFunction_t *instance)
 {
@@ -51,7 +53,11 @@ void pgResetFn_globalFunctions(globalFunction_t *instance)
             .enabled = 0,
             .conditionId = -1,
             .action = 0,
-            .withValue = {
+            .withValueA = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .withValueB = {
                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
                 .value = 0
             },
@@ -68,9 +74,13 @@ void globalFunctionsProcess(int8_t functionId) {
         const int previousValue = globalFunctionsStates[functionId].active;
 
         globalFunctionsStates[functionId].active = (bool) conditionValue;
-        globalFunctionsStates[functionId].value = logicConditionGetOperandValue(
-            globalFunctions(functionId)->withValue.type,
-            globalFunctions(functionId)->withValue.value
+        globalFunctionsStates[functionId].valueA = logicConditionGetOperandValue(
+            globalFunctions(functionId)->withValueA.type,
+            globalFunctions(functionId)->withValueA.value
+        );
+        globalFunctionsStates[functionId].valueB = logicConditionGetOperandValue(
+            globalFunctions(functionId)->withValueB.type,
+            globalFunctions(functionId)->withValueB.value
         );
 
         switch (globalFunctions(functionId)->action) {
@@ -81,7 +91,7 @@ void globalFunctionsProcess(int8_t functionId) {
                 break;
             case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE:
                 if (conditionValue) {
-                    globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] = globalFunctionsStates[functionId].value;
+                    globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] = globalFunctionsStates[functionId].valueA;
                     GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE);
                 }
                 break;
@@ -94,23 +104,23 @@ void globalFunctionsProcess(int8_t functionId) {
                 if (conditionValue && !previousValue) {
                     vtxDeviceCapability_t vtxDeviceCapability;
                     if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
-                        vtxSettingsConfigMutable()->power = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
+                        vtxSettingsConfigMutable()->power = constrain(globalFunctionsStates[functionId].valueA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
                     }
                 }
                 break;
             case GLOBAL_FUNCTION_ACTION_SET_VTX_BAND:
                 if (conditionValue && !previousValue) {
                     vtxDeviceCapability_t vtxDeviceCapability;
                     if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
-                        vtxSettingsConfigMutable()->band = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
+                        vtxSettingsConfigMutable()->band = constrain(globalFunctionsStates[functionId].valueA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
                     }
                 }
                 break;
             case GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL:
                 if (conditionValue && !previousValue) {
                     vtxDeviceCapability_t vtxDeviceCapability;
                     if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
-                        vtxSettingsConfigMutable()->channel = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
+                        vtxSettingsConfigMutable()->channel = constrain(globalFunctionsStates[functionId].valueA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
                     }
                 }
                 break;
@@ -131,10 +141,19 @@ void globalFunctionsProcess(int8_t functionId) {
                 break;
             case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE:
                 if (conditionValue) {
-                    globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].value;
+                    globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].valueA;
                     GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE);
                 }
                 break;
+            case GLOBAL_FUNCTION_ACTION_OVERRIDE_RC_CHANNEL:
+                if (conditionValue) {
+                    // globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].valueA;
+                    const uint8_t channel = constrain(globalFunctionsStates[functionId].valueA, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1);
+                    channelOverrides[channel].active = true;
+                    channelOverrides[channel].value = constrain(globalFunctionsStates[functionId].valueB, PWM_RANGE_MIN, PWM_RANGE_MAX);
+                    GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_RC_CHANNEL);
+                }
+                break;
         }
     }
 }
@@ -145,11 +164,23 @@ void NOINLINE globalFunctionsUpdateTask(timeUs_t currentTimeUs) {
     //Disable all flags
     globalFunctionsFlags = 0;
 
+    for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
+        channelOverrides[i].active = false;
+    } 
+
     for (uint8_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
         globalFunctionsProcess(i);
     }
 }
 
+int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
+    if (channelOverrides[channel].active) {
+        return channelOverrides[channel].value;
+    } else {
+        return originalValue;
+    }
+}
+
 float NOINLINE getThrottleScale(float globalThrottleScale) {
     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE)) {
         return constrainf(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] / 100.0f, 0.0f, 1.0f);

diff --git a/src/main/common/global_functions.h b/src/main/common/global_functions.h
@@ -39,6 +39,7 @@ typedef enum {
     GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,               // 7
     GLOBAL_FUNCTION_ACTION_SET_VTX_BAND,                    // 8
     GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL,                 // 9
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_RC_CHANNEL,             // 10
     GLOBAL_FUNCTION_ACTION_LAST
 } globalFunctionActions_e;
 
@@ -50,22 +51,30 @@ typedef enum {
     GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4),
     GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
     GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE = (1 << 6),
+    GLOBAL_FUNCTION_FLAG_OVERRIDE_RC_CHANNEL = (1 << 7),
 } globalFunctionFlags_t;
 
 typedef struct globalFunction_s {
     uint8_t enabled;
     int8_t conditionId;
     uint8_t action;
-    logicOperand_t withValue;
+    logicOperand_t withValueA;
+    logicOperand_t withValueB;
     uint8_t flags;
 } globalFunction_t;
 
 typedef struct globalFunctionState_s {
     uint8_t active;
-    int value;
+    int valueA;
+    int valueB;
     uint8_t flags;
 } globalFunctionState_t;
 
+typedef struct rcChannelOverride_s {
+    uint8_t active;
+    int value;
+} rcChannelOverride_t;
+
 extern uint64_t globalFunctionsFlags;
 
 #define GLOBAL_FUNCTION_FLAG_DISABLE(mask) (globalFunctionsFlags &= ~(mask))
@@ -77,4 +86,5 @@ extern int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST];
 
 void globalFunctionsUpdateTask(timeUs_t currentTimeUs);
 float getThrottleScale(float globalThrottleScale);
-int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
+int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
+int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
@@ -1945,7 +1945,7 @@ static void cliGvar(char *cmdline) {
 
 static void printGlobalFunctions(uint8_t dumpMask, const globalFunction_t *globalFunctions, const globalFunction_t *defaultGlobalFunctions)
 {
-    const char *format = "gf %d %d %d %d %d %d %d";
+    const char *format = "gf %d %d %d %d %d %d %d %d %d";
     for (uint32_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
         const globalFunction_t gf = globalFunctions[i];
 
@@ -1956,17 +1956,21 @@ static void printGlobalFunctions(uint8_t dumpMask, const globalFunction_t *globa
                 gf.enabled == defaultValue.enabled &&
                 gf.conditionId == defaultValue.conditionId &&
                 gf.action == defaultValue.action &&
-                gf.withValue.type == defaultValue.withValue.type &&
-                gf.withValue.value == defaultValue.withValue.value &&
+                gf.withValueA.type == defaultValue.withValueA.type &&
+                gf.withValueA.value == defaultValue.withValueA.value &&
+                gf.withValueB.type == defaultValue.withValueB.type &&
+                gf.withValueB.value == defaultValue.withValueB.value &&
                 gf.flags == defaultValue.flags;
 
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
                 gf.enabled,
                 gf.conditionId,
                 gf.action,
-                gf.withValue.type,
-                gf.withValue.value,
+                gf.withValueA.type,
+                gf.withValueA.value,
+                gf.withValueB.type,
+                gf.withValueB.value,
                 gf.flags
             );
         }
@@ -1975,16 +1979,18 @@ static void printGlobalFunctions(uint8_t dumpMask, const globalFunction_t *globa
             gf.enabled,
             gf.conditionId,
             gf.action,
-            gf.withValue.type,
-            gf.withValue.value,
+            gf.withValueA.type,
+            gf.withValueA.value,
+            gf.withValueB.type,
+            gf.withValueB.value,
             gf.flags
         );
     }
 }
 
 static void cliGlobalFunctions(char *cmdline) {
     char * saveptr;
-    int args[7], check = 0;
+    int args[9], check = 0;
     uint8_t len = strlen(cmdline);
 
     if (len == 0) {
@@ -1997,8 +2003,10 @@ static void cliGlobalFunctions(char *cmdline) {
             ENABLED,
             CONDITION_ID,
             ACTION,
-            VALUE_TYPE,
-            VALUE_VALUE,
+            VALUE_TYPE_A,
+            VALUE_VALUE_A,
+            VALUE_TYPE_B,
+            VALUE_VALUE_B,
             FLAGS,
             ARGS_COUNT
             };
@@ -2019,16 +2027,20 @@ static void cliGlobalFunctions(char *cmdline) {
             args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
             args[CONDITION_ID] >= -1 && args[CONDITION_ID] < MAX_LOGIC_CONDITIONS &&
             args[ACTION] >= 0 && args[ACTION] < GLOBAL_FUNCTION_ACTION_LAST &&
-            args[VALUE_TYPE] >= 0 && args[VALUE_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
-            args[VALUE_VALUE] >= -1000000 && args[VALUE_VALUE] <= 1000000 &&
+            args[VALUE_TYPE_A] >= 0 && args[VALUE_TYPE_A] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[VALUE_VALUE_A] >= -1000000 && args[VALUE_VALUE_A] <= 1000000 &&
+            args[VALUE_TYPE_B] >= 0 && args[VALUE_TYPE_B] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[VALUE_VALUE_B] >= -1000000 && args[VALUE_VALUE_B] <= 1000000 &&
             args[FLAGS] >= 0 && args[FLAGS] <= 255
 
         ) {
             globalFunctionsMutable(i)->enabled = args[ENABLED];
             globalFunctionsMutable(i)->conditionId = args[CONDITION_ID];
             globalFunctionsMutable(i)->action = args[ACTION];
-            globalFunctionsMutable(i)->withValue.type = args[VALUE_TYPE];
-            globalFunctionsMutable(i)->withValue.value = args[VALUE_VALUE];
+            globalFunctionsMutable(i)->withValueA.type = args[VALUE_TYPE_A];
+            globalFunctionsMutable(i)->withValueA.value = args[VALUE_VALUE_A];
+            globalFunctionsMutable(i)->withValueB.type = args[VALUE_TYPE_B];
+            globalFunctionsMutable(i)->withValueB.value = args[VALUE_VALUE_B];
             globalFunctionsMutable(i)->flags = args[FLAGS];
 
             cliGlobalFunctions("");
@@ -3562,7 +3574,7 @@ const clicmd_t cmdTable[] = {
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
     CLI_COMMAND_DEF("gf", "configure global functions",
-        "<rule> <enabled> <logic condition> <action> <operand type> <operand value> <flags>\r\n"
+        "<rule> <enabled> <logic condition> <action> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
         "\treset\r\n", cliGlobalFunctions),
 #endif
     CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -556,8 +556,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU8(dst, globalFunctions(i)->enabled);
             sbufWriteU8(dst, globalFunctions(i)->conditionId);
             sbufWriteU8(dst, globalFunctions(i)->action);
-            sbufWriteU8(dst, globalFunctions(i)->withValue.type);
-            sbufWriteU32(dst, globalFunctions(i)->withValue.value);
+            sbufWriteU8(dst, globalFunctions(i)->withValueA.type);
+            sbufWriteU32(dst, globalFunctions(i)->withValueA.value);
+            sbufWriteU8(dst, globalFunctions(i)->withValueB.type);
+            sbufWriteU32(dst, globalFunctions(i)->withValueB.value);
             sbufWriteU8(dst, logicConditions(i)->flags);
         }
         break;
@@ -1966,12 +1968,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_GLOBAL_FUNCTIONS
     case MSP2_INAV_SET_GLOBAL_FUNCTIONS:
         sbufReadU8Safe(&tmp_u8, src);
-        if ((dataSize == 10) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
+        if ((dataSize == 15) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
             globalFunctionsMutable(tmp_u8)->enabled = sbufReadU8(src);
             globalFunctionsMutable(tmp_u8)->conditionId = sbufReadU8(src);
             globalFunctionsMutable(tmp_u8)->action = sbufReadU8(src);
-            globalFunctionsMutable(tmp_u8)->withValue.type = sbufReadU8(src);
-            globalFunctionsMutable(tmp_u8)->withValue.value = sbufReadU32(src);
+            globalFunctionsMutable(tmp_u8)->withValueA.type = sbufReadU8(src);
+            globalFunctionsMutable(tmp_u8)->withValueA.value = sbufReadU32(src);
+            globalFunctionsMutable(tmp_u8)->withValueB.type = sbufReadU8(src);
+            globalFunctionsMutable(tmp_u8)->withValueB.value = sbufReadU32(src);
             globalFunctionsMutable(tmp_u8)->flags = sbufReadU8(src);
         } else
             return MSP_RESULT_ERROR;

diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
@@ -28,6 +28,7 @@
 
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/global_functions.h"
 
 #include "config/feature.h"
 #include "config/parameter_group.h"
@@ -695,7 +696,11 @@ uint16_t rxGetRefreshRate(void)
 
 int16_t rxGetChannelValue(unsigned channelNumber)
 {
-    return rcChannels[channelNumber].data;
+    if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_RC_CHANNEL)) {
+        return getRcChannelOverride(channelNumber, rcChannels[channelNumber].data);
+    } else {
+        return rcChannels[channelNumber].data;
+    }
 }
 
 int16_t rxGetRawChannelValue(unsigned channelNumber)