diff --git a/make/source.mk b/make/source.mk index 6064cb548f..45dfb778f0 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 0f52c2f010..1a56c0b472 100644 --- a/src/main/common/filter.c +++ b/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; diff --git a/src/main/common/global_functions.c b/src/main/common/global_functions.c new file mode 100644 index 0000000000..8771245af4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/main/common/global_functions.h b/src/main/common/global_functions.h new file mode 100644 index 0000000000..d49894304a --- /dev/null +++ b/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); \ No newline at end of file diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index d14f815a49..53c31c3124 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 95c56db8bb..129afac484 100644 --- a/src/main/drivers/pwm_output.c +++ b/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(); } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 7affc2b1e2..be3a1acc91 100755 --- a/src/main/fc/cli.c +++ b/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", " \r\n" "\treset\r\n", cliLogic), +#endif +#ifdef USE_GLOBAL_FUNCTIONS + CLI_COMMAND_DEF("gf", "configure global functions", + " \r\n" + "\treset\r\n", cliGlobalFunctions), #endif CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), CLI_COMMAND_DEF("smix", "servo mixer", diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index c04fe5e42f..7974f88f1b 100755 --- a/src/main/fc/fc_core.c +++ b/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(); - - if (!isArmingDisabled() || emergencyArmingIsEnabled()) { +#ifdef USE_GLOBAL_FUNCTIONS + if ( + !isArmingDisabled() || + emergencyArmingIsEnabled() || + GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY) + ) { +#else + if ( + !isArmingDisabled() || + emergencyArmingIsEnabled() + ) { +#endif if (ARMING_FLAG(ARMED)) { return; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index fe94ada9b6..ae28348f48 100644 --- a/src/main/fc/fc_msp.c +++ b/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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a71bf2692f..553c348ab6 100755 --- a/src/main/fc/fc_tasks.c +++ b/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 }; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d1b2e854ea..36ab742c57 100755 --- a/src/main/flight/mixer.c +++ b/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); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 1dbcf66a45..fd6a7c5e27 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/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 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index cf42a7ea5f..5cf996f02e 100755 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/target/common.h b/src/main/target/common.h index 742bb07570..e84c13ae26 100755 --- a/src/main/target/common.h +++ b/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