diff --git a/make/source.mk b/make/source.mk index 4fb7f2618a..e59c2afd30 100644 --- a/make/source.mk +++ b/make/source.mk @@ -16,6 +16,7 @@ COMMON_SRC = \ common/log.c \ common/logic_condition.c \ common/global_functions.c \ + common/global_variables.c \ common/maths.c \ common/memory.c \ common/olc.c \ diff --git a/src/main/common/global_variables.c b/src/main/common/global_variables.c new file mode 100644 index 0000000000..a07d7f09df --- /dev/null +++ b/src/main/common/global_variables.c @@ -0,0 +1,49 @@ +/* + * 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 "platform.h" + +#ifdef USE_LOGIC_CONDITIONS + +#include +#include "common/global_variables.h" +#include "build/build_config.h" + +static EXTENDED_FASTRAM int globalVariableState[MAX_GLOBAL_VARIABLES]; + +int gvGet(uint8_t index) { + if (index < MAX_GLOBAL_VARIABLES) { + return globalVariableState[index]; + } else { + return 0; + } +} + +void gvSet(uint8_t index, int value) { + if (index < MAX_GLOBAL_VARIABLES) { + globalVariableState[index] = value; + } +} + +#endif \ No newline at end of file diff --git a/src/main/common/global_variables.h b/src/main/common/global_variables.h new file mode 100644 index 0000000000..bebfa6904b --- /dev/null +++ b/src/main/common/global_variables.h @@ -0,0 +1,30 @@ +/* + * 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 + +#define MAX_GLOBAL_VARIABLES 4 + +int gvGet(uint8_t index); +void gvSet(uint8_t index, int value); \ No newline at end of file diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c index 14d6cadcaa..062524740b 100644 --- a/src/main/common/logic_condition.c +++ b/src/main/common/logic_condition.c @@ -29,6 +29,7 @@ #include "config/parameter_group_ids.h" #include "common/logic_condition.h" +#include "common/global_variables.h" #include "common/utils.h" #include "rx/rx.h" #include "maths.h" @@ -50,6 +51,7 @@ static int logicConditionCompute( int operandA, int operandB ) { + int temporaryValue; switch (operation) { case LOGIC_CONDITION_TRUE: @@ -118,6 +120,43 @@ static int logicConditionCompute( return currentVaue; break; + case LOGIC_CONDITION_GVAR_SET: + gvSet(operandA, operandB); + return operandB; + break; + + case LOGIC_CONDITION_GVAR_INC: + temporaryValue = gvGet(operandA) + operandB; + gvSet(operandA, temporaryValue); + return temporaryValue; + break; + + case LOGIC_CONDITION_GVAR_DEC: + temporaryValue = gvGet(operandA) - operandB; + gvSet(operandA, temporaryValue); + return temporaryValue; + break; + + case LOGIC_CONDITION_ADD: + return operandA + operandB; + break; + + case LOGIC_CONDITION_SUB: + return operandA - operandB; + break; + + case LOGIC_CONDITION_MUL: + return operandA * operandB; + break; + + case LOGIC_CONDITION_DIV: + if (operandB != 0) { + return operandA / operandB; + } else { + return operandA; + } + break; + default: return false; break; @@ -314,6 +353,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { } break; + case LOGIC_CONDITION_OPERAND_TYPE_GVAR: + if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) { + retVal = gvGet(operand); + } + break; + default: break; } diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h index df6518a4c5..737d52a92a 100644 --- a/src/main/common/logic_condition.h +++ b/src/main/common/logic_condition.h @@ -43,6 +43,13 @@ typedef enum { LOGIC_CONDITION_NOR, // 11 LOGIC_CONDITION_NOT, // 12 LOGIC_CONDITION_STICKY, // 13 + LOGIC_CONDITION_ADD, // 14 + LOGIC_CONDITION_SUB, // 15 + LOGIC_CONDITION_MUL, // 16 + LOGIC_CONDITION_DIV, // 17 + LOGIC_CONDITION_GVAR_SET, // 18 + LOGIC_CONDITION_GVAR_INC, // 19 + LOGIC_CONDITION_GVAR_DEC, // 21 LOGIC_CONDITION_LAST } logicOperation_e; @@ -52,6 +59,7 @@ typedef enum logicOperandType_s { LOGIC_CONDITION_OPERAND_TYPE_FLIGHT, LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE, LOGIC_CONDITION_OPERAND_TYPE_LC, // Result of different LC and LC operand + LOGIC_CONDITION_OPERAND_TYPE_GVAR, // Value from a global variable LOGIC_CONDITION_OPERAND_TYPE_LAST } logicOperandType_e; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 3ddf770afe..aa78812c43 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -28,6 +28,7 @@ #include "common/axis.h" #include "common/filter.h" #include "common/maths.h" +#include "common/global_variables.h" #include "config/config_reset.h" #include "config/feature.h" @@ -263,6 +264,12 @@ void servoMixer(float dT) input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0; input[INPUT_LOGIC_ONE] = 500; +#ifdef USE_LOGIC_CONDITIONS + input[INPUT_GVAR_0] = constrain(gvGet(0), -1000, 1000); + input[INPUT_GVAR_1] = constrain(gvGet(1), -1000, 1000); + input[INPUT_GVAR_2] = constrain(gvGet(2), -1000, 1000); + input[INPUT_GVAR_3] = constrain(gvGet(3), -1000, 1000); +#endif if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -900, 900, -500, +500); diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 3025bc4edc..8745cf239d 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -54,6 +54,10 @@ typedef enum { INPUT_STABILIZED_YAW_PLUS = 27, INPUT_STABILIZED_YAW_MINUS = 28, INPUT_LOGIC_ONE = 29, + INPUT_GVAR_0 = 30, + INPUT_GVAR_1 = 31, + INPUT_GVAR_2 = 32, + INPUT_GVAR_3 = 33, INPUT_SOURCE_COUNT } inputSource_e;