Global Functions (#4474)

* First cut on data structure

* fix source.mk

* Dummy task to process global functions

* Early stage of arming safety override

* CLI interface for global functions

* MSP layer for global functions

* arming safety override with a global function

* Integrate throttle limit into global functions

* Fix Omnibus RAM overflow

make/source.mk

@@ -15,6 +15,7 @@ COMMON_SRC = \
             common/gps_conversion.c \
             common/log.c \
             common/logic_condition.c \
+            common/global_functions.c \
             common/maths.c \
             common/memory.c \
             common/olc.c \

src/main/common/filter.c

@@ -200,7 +200,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 }
 
 // Computes a biquad_t filter on a sample
-float FAST_CODE biquadFilterApply(biquadFilter_t *filter, float input)
+float FAST_CODE NOINLINE biquadFilterApply(biquadFilter_t *filter, float input)
 {
     const float result = filter->b0 * input + filter->d1;
     filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;

src/main/common/global_functions.c

@@ -0,0 +1,105 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "common/utils.h"
+#include "common/maths.h"
+#include "common/global_functions.h"
+#include "common/logic_condition.h"
+
+#ifdef USE_GLOBAL_FUNCTIONS
+
+PG_REGISTER_ARRAY(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
+
+EXTENDED_FASTRAM uint64_t globalFunctionsFlags = 0;
+EXTENDED_FASTRAM globalFunctionState_t globalFunctionsStates[MAX_GLOBAL_FUNCTIONS];
+EXTENDED_FASTRAM int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST];
+
+void pgResetFn_globalFunctions(globalFunction_t *instance)
+{
+    for (int i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
+        RESET_CONFIG(globalFunction_t, &instance[i],
+            .enabled = 0,
+            .conditionId = -1,
+            .action = 0,
+            .withValue = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .flags = 0
+        );
+    }
+}
+
+void globalFunctionsProcess(int8_t functionId) {
+    //Process only activated functions
+    if (globalFunctions(functionId)->enabled) {
+
+        const int conditionValue = logicConditionGetValue(globalFunctions(functionId)->conditionId);
+
+        globalFunctionsStates[functionId].active = (bool) conditionValue;
+        globalFunctionsStates[functionId].value = logicConditionGetOperandValue(
+            globalFunctions(functionId)->withValue.type,
+            globalFunctions(functionId)->withValue.value
+        );
+
+        switch (globalFunctions(functionId)->action) {
+            case GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY:
+                if (conditionValue) {
+                    GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY);
+                }
+                break;
+            case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE:
+                if (conditionValue) {
+                    globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] = globalFunctionsStates[functionId].value;
+                    GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE);
+                }
+                break;
+        }
+    }
+}
+
+void NOINLINE globalFunctionsUpdateTask(timeUs_t currentTimeUs) {
+    UNUSED(currentTimeUs);
+
+    //Disable all flags
+    globalFunctionsFlags = 0;
+
+    for (uint8_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
+        globalFunctionsProcess(i);
+    }
+}
+
+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);
+    } else {
+        return globalThrottleScale;
+    }
+}
+
+#endif

src/main/common/global_functions.h

@@ -0,0 +1,65 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+#pragma once
+
+#include "config/parameter_group.h"
+#include "common/logic_condition.h"
+
+#define MAX_GLOBAL_FUNCTIONS 8
+
+typedef enum {
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY = 0,
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE,
+    GLOBAL_FUNCTION_ACTION_LAST
+} globalFunctionActions_e;
+
+typedef enum {
+    GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0),
+    GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),
+} globalFunctionFlags_t;
+
+typedef struct globalFunction_s {
+    uint8_t enabled;
+    int8_t conditionId;
+    uint8_t action;
+    logicOperand_t withValue;
+    uint8_t flags;
+} globalFunction_t;
+
+typedef struct globalFunctionState_s {
+    uint8_t active;
+    int value;
+    uint8_t flags;
+} globalFunctionState_t;
+
+extern uint64_t globalFunctionsFlags;
+
+#define GLOBAL_FUNCTION_FLAG_DISABLE(mask) (globalFunctionsFlags &= ~(mask))
+#define GLOBAL_FUNCTION_FLAG_ENABLE(mask) (globalFunctionsFlags |= (mask))
+#define GLOBAL_FUNCTION_FLAG(mask) (globalFunctionsFlags & (mask))
+
+PG_DECLARE_ARRAY(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions);
+
+void globalFunctionsUpdateTask(timeUs_t currentTimeUs);
+float getThrottleScale(float globalThrottleScale);

src/main/config/parameter_group_ids.h

@@ -107,7 +107,8 @@
 #define PG_LOG_CONFIG 1017
 #define PG_RCDEVICE_CONFIG 1018
 #define PG_GENERAL_SETTINGS 1019
-#define PG_INAV_END 1019
+#define PG_GLOBAL_FUNCTIONS 1020
+#define PG_INAV_END 1020
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/pwm_output.c

@@ -304,19 +304,19 @@ static void pwmWriteDigital(uint8_t index, uint16_t value)
     motors[index].value = constrain(value, 0, 2047);
 }
 
-bool FAST_CODE NOINLINE isMotorProtocolDshot(void)
+bool isMotorProtocolDshot(void)
 {
     // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config
     // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used
     return getMotorProtocolProperties(initMotorProtocol)->isDSHOT;
 }
 
-bool FAST_CODE NOINLINE isMotorProtocolSerialShot(void)
+bool isMotorProtocolSerialShot(void)
 {
     return getMotorProtocolProperties(initMotorProtocol)->isSerialShot;
 }
 
-bool FAST_CODE NOINLINE isMotorProtocolDigital(void)
+bool isMotorProtocolDigital(void)
 {
     return isMotorProtocolDshot() || isMotorProtocolSerialShot();
 }

src/main/fc/cli.c

@@ -43,6 +43,7 @@ extern uint8_t __config_end;
 #include "common/memory.h"
 #include "common/time.h"
 #include "common/typeconversion.h"
+#include "common/global_functions.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -1777,7 +1778,7 @@ static void cliLogic(char *cmdline) {
     if (len == 0) {
         printLogic(DUMP_MASTER, logicConditions(0), NULL);
     } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
-        pgResetCopy(logicConditionsMutable(0), PG_SERVO_MIXER);
+        pgResetCopy(logicConditionsMutable(0), PG_LOGIC_CONDITIONS);
     } else {
         enum {
             INDEX = 0,
@@ -1829,6 +1830,104 @@ static void cliLogic(char *cmdline) {
 }
 #endif
 
+#ifdef USE_GLOBAL_FUNCTIONS
+
+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";
+    for (uint32_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
+        const globalFunction_t gf = globalFunctions[i];
+
+        bool equalsDefault = false;
+        if (defaultGlobalFunctions) {
+            globalFunction_t defaultValue = defaultGlobalFunctions[i];
+            equalsDefault =
+                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.flags == defaultValue.flags;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                gf.enabled,
+                gf.conditionId,
+                gf.action,
+                gf.withValue.type,
+                gf.withValue.value,
+                gf.flags
+            );
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            gf.enabled,
+            gf.conditionId,
+            gf.action,
+            gf.withValue.type,
+            gf.withValue.value,
+            gf.flags
+        );
+    }
+}
+
+static void cliGlobalFunctions(char *cmdline) {
+    char * saveptr;
+    int args[7], check = 0;
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printGlobalFunctions(DUMP_MASTER, globalFunctions(0), NULL);
+    } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
+        pgResetCopy(globalFunctionsMutable(0), PG_GLOBAL_FUNCTIONS);
+    } else {
+        enum {
+            INDEX = 0,
+            ENABLED,
+            CONDITION_ID,
+            ACTION,
+            VALUE_TYPE,
+            VALUE_VALUE,
+            FLAGS,
+            ARGS_COUNT
+            };
+        char *ptr = strtok_r(cmdline, " ", &saveptr);
+        while (ptr != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptr);
+            ptr = strtok_r(NULL, " ", &saveptr);
+        }
+
+        if (ptr != NULL || check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        int32_t i = args[INDEX];
+        if (
+            i >= 0 && i < MAX_GLOBAL_FUNCTIONS &&
+            args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
+            args[CONDITION_ID] >= 0 && 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[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)->flags = args[FLAGS];
+
+            cliGlobalFunctions("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+#endif
+
 #ifdef USE_SDCARD
 
 static void cliWriteBytes(const uint8_t *buffer, int count)
@@ -3064,6 +3163,11 @@ static void printConfig(const char *cmdline, bool doDiff)
         printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0));
 #endif
 
+#ifdef USE_GLOBAL_FUNCTIONS
+        cliPrintHashLine("gf");
+        printGlobalFunctions(dumpMask, globalFunctions_CopyArray, globalFunctions(0));
+#endif
+
         cliPrintHashLine("feature");
         printFeature(dumpMask, &featureConfig_Copy, featureConfig());
 
@@ -3275,6 +3379,11 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("logic", "configure logic conditions",
         "<rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
         "\treset\r\n", cliLogic),
+#endif
+#ifdef USE_GLOBAL_FUNCTIONS
+    CLI_COMMAND_DEF("gf", "configure global functions",
+        "<rule> <enabled> <logic condition> <action> <operand type> <operand value> <flags>\r\n"
+        "\treset\r\n", cliGlobalFunctions),
 #endif
     CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
     CLI_COMMAND_DEF("smix", "servo mixer",

src/main/fc/fc_core.c

@@ -30,6 +30,7 @@
 #include "common/color.h"
 #include "common/utils.h"
 #include "common/filter.h"
+#include "common/global_functions.h"
 
 #include "drivers/light_led.h"
 #include "drivers/serial.h"
@@ -442,8 +443,18 @@ void releaseSharedTelemetryPorts(void) {
 void tryArm(void)
 {
     updateArmingStatus();
-
+#ifdef USE_GLOBAL_FUNCTIONS
-    if (!isArmingDisabled() || emergencyArmingIsEnabled()) {
+    if (
+        !isArmingDisabled() || 
+        emergencyArmingIsEnabled() || 
+        GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY)
+    ) {
+#else 
+    if (
+        !isArmingDisabled() || 
+        emergencyArmingIsEnabled()
+    ) {
+#endif
         if (ARMING_FLAG(ARMED)) {
             return;
         }

src/main/fc/fc_msp.c

@@ -35,6 +35,7 @@
 #include "common/bitarray.h"
 #include "common/time.h"
 #include "common/utils.h"
+#include "common/global_functions.h"
 
 #include "config/parameter_group_ids.h"
 
@@ -495,6 +496,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU32(dst, logicConditionGetValue(i));
         }
         break;
+#endif
+#ifdef USE_GLOBAL_FUNCTIONS
+    case MSP2_INAV_GLOBAL_FUNCTIONS:
+        for (int i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
+            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, logicConditions(i)->flags);
+        }
+        break;
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@@ -1919,6 +1932,20 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         } else
             return MSP_RESULT_ERROR;
         break;
+#endif
+#ifdef USE_GLOBAL_FUNCTIONS
+    case MSP2_INAV_SET_GLOBAL_FUNCTIONS:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == 14) && (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)->flags = sbufReadU8(src);
+        } else
+            return MSP_RESULT_ERROR;
+        break;
 #endif
     case MSP2_COMMON_SET_MOTOR_MIXER:
         sbufReadU8Safe(&tmp_u8, src);

src/main/fc/fc_tasks.c

@@ -27,6 +27,7 @@
 #include "common/color.h"
 #include "common/utils.h"
 #include "common/logic_condition.h"
+#include "common/global_functions.h"
 
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/compass/compass.h"
@@ -339,6 +340,9 @@ void fcTasksInit(void)
 #ifdef USE_LOGIC_CONDITIONS
     setTaskEnabled(TASK_LOGIC_CONDITIONS, true);
 #endif
+#ifdef USE_GLOBAL_FUNCTIONS
+    setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -552,4 +556,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+#ifdef USE_GLOBAL_FUNCTIONS
+    [TASK_GLOBAL_FUNCTIONS] = {
+        .taskName = "G_FNK",
+        .taskFunc = globalFunctionsUpdateTask,
+        .desiredPeriod = TASK_PERIOD_HZ(10),          // 10Hz @100msec
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
 };

src/main/flight/mixer.c

@@ -27,6 +27,7 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/global_functions.h"
 
 #include "config/feature.h"
 #include "config/parameter_group.h"
@@ -343,8 +344,11 @@ void FAST_CODE NOINLINE mixTable(const float dT)
         throttleMax = motorConfig()->maxthrottle;
 
         // Throttle scaling to limit max throttle when battery is full
+    #ifdef USE_GLOBAL_FUNCTIONS
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleMin;
+    #else
         mixerThrottleCommand = ((mixerThrottleCommand - throttleMin) * motorConfig()->throttleScale) + throttleMin;
-
+    #endif
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {                
             mixerThrottleCommand = MIN(throttleMin + (mixerThrottleCommand - throttleMin) * calculateThrottleCompensationFactor(), throttleMax);

src/main/msp/msp_protocol_v2_inav.h

@@ -57,9 +57,10 @@
 #define MSP2_INAV_SET_SERVO_MIXER               0x2021
 #define MSP2_INAV_LOGIC_CONDITIONS              0x2022
 #define MSP2_INAV_SET_LOGIC_CONDITIONS          0x2023
+#define MSP2_INAV_GLOBAL_FUNCTIONS              0x2024
+#define MSP2_INAV_SET_GLOBAL_FUNCTIONS          0x2025
 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS       0x2026
 
-
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
 

src/main/scheduler/scheduler.h

@@ -112,6 +112,9 @@ typedef enum {
 #endif
 #ifdef USE_LOGIC_CONDITIONS
     TASK_LOGIC_CONDITIONS,
+#endif
+#ifdef USE_GLOBAL_FUNCTIONS
+    TASK_GLOBAL_FUNCTIONS,
 #endif
     /* Count of real tasks */
     TASK_COUNT,

src/main/target/common.h

@@ -157,7 +157,7 @@
 
 #ifndef STM32F3 //F3 series does not have enoug RAM to support logic conditions
 #define USE_LOGIC_CONDITIONS
-
+#define USE_GLOBAL_FUNCTIONS
 #define USE_CLI_BATCH
 #endif