Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-15 12:25:17 +03:00
Code Issues Wiki Activity

Unify Global Functions and Logic Conditions into single entity

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-07-22 21:53:54 +02:00
parent 30d1323c31
commit 827ac04950
8 changed files with 269 additions and 138 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/fc_core.c
View file

@ -450,7 +450,7 @@ void tryArm(void)
if (
!isArmingDisabled() ||
emergencyArmingIsEnabled() ||
GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY)
LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)
) {
#else
if (

4
src/main/flight/mixer.c
View file

@ -480,10 +480,10 @@ void FAST_CODE mixTable(const float dT)
// Find min and max throttle based on condition.
#ifdef USE_PROGRAMMING_FRAMEWORK
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) {
throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle;
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
} else
#endif
if (feature(FEATURE_REVERSIBLE_MOTORS)) {

4
src/main/io/osd.c
View file

@ -3243,8 +3243,8 @@ void osdUpdate(timeUs_t currentTimeUs)
activeLayout = 1;
else
#ifdef USE_PROGRAMMING_FRAMEWORK
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT))
activeLayout = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT);
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT))
activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT);
else
#endif
activeLayout = 0;

218
src/main/programming/global_functions.c
View file

@ -40,9 +40,9 @@
PG_REGISTER_ARRAY_WITH_RESET_FN(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];
// 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)
{
@ -64,126 +64,118 @@ void globalFunctionsProcess(int8_t functionId) {
//Process only activated functions
if (globalFunctions(functionId)->enabled) {
const int conditionValue = logicConditionGetValue(globalFunctions(functionId)->conditionId);
const int previousValue = globalFunctionsStates[functionId].active;
// const int conditionValue = logicConditionGetValue(globalFunctions(functionId)->conditionId);
// 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].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;
case GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW:
if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW);
}
break;
case GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL:
if (conditionValue && !previousValue) {
vtxDeviceCapability_t vtxDeviceCapability;
if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
vtxSettingsConfigMutable()->power = constrain(globalFunctionsStates[functionId].value, 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);
}
}
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);
}
}
break;
case GLOBAL_FUNCTION_ACTION_INVERT_ROLL:
if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL);
}
break;
case GLOBAL_FUNCTION_ACTION_INVERT_PITCH:
if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH);
}
break;
case GLOBAL_FUNCTION_ACTION_INVERT_YAW:
if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW);
}
break;
case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE:
if (conditionValue) {
globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE);
}
break;
case GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT:
if(conditionValue){
globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT);
}
break;
}
// switch (globalFunctions(functionId)->action) {
// case GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY:
// if (conditionValue) {
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE:
// if (conditionValue) {
// globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] = globalFunctionsStates[functionId].value;
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW:
// if (conditionValue) {
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL:
// if (conditionValue && !previousValue) {
// vtxDeviceCapability_t vtxDeviceCapability;
// if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
// vtxSettingsConfigMutable()->power = constrain(globalFunctionsStates[functionId].value, 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);
// }
// }
// 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);
// }
// }
// break;
// case GLOBAL_FUNCTION_ACTION_INVERT_ROLL:
// if (conditionValue) {
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_INVERT_PITCH:
// if (conditionValue) {
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_INVERT_YAW:
// if (conditionValue) {
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE:
// if (conditionValue) {
// globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].value;
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
// }
// break;
// case GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT:
// if(conditionValue){
// globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT] = globalFunctionsStates[functionId].value;
// LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
// }
// break;
// }
}
}
void NOINLINE globalFunctionsUpdateTask(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
// void NOINLINE globalFunctionsUpdateTask(timeUs_t currentTimeUs) {
// UNUSED(currentTimeUs);
//Disable all flags
globalFunctionsFlags = 0;
// //Disable all flags
// globalFunctionsFlags = 0;
for (uint8_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
globalFunctionsProcess(i);
}
}
// 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;
}
}
// int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
// int16_t outputValue = command[axis];
int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
int16_t outputValue = command[axis];
// if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
// outputValue = command[FD_YAW];
// } else if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_YAW) {
// outputValue = command[FD_ROLL];
// }
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
outputValue = command[FD_YAW];
} else if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_YAW) {
outputValue = command[FD_ROLL];
}
// if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
// outputValue *= -1;
// }
// if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH) && axis == FD_PITCH) {
// outputValue *= -1;
// }
// if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW) && axis == FD_YAW) {
// outputValue *= -1;
// }
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
outputValue *= -1;
}
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH) && axis == FD_PITCH) {
outputValue *= -1;
}
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW) && axis == FD_YAW) {
outputValue *= -1;
}
return outputValue;
}
// return outputValue;
// }
#endif

23
src/main/programming/global_functions.h
View file

@ -43,17 +43,6 @@ typedef enum {
GLOBAL_FUNCTION_ACTION_LAST
} globalFunctionActions_e;
typedef enum {
GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0),
GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),
GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW = (1 << 2),
GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL = (1 << 3),
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_OSD_LAYOUT = (1 << 7),
} globalFunctionFlags_t;
typedef struct globalFunction_s {
uint8_t enabled;
int8_t conditionId;
@ -70,13 +59,9 @@ typedef struct globalFunctionState_s {
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);
extern int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST];
// 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);
// void globalFunctionsUpdateTask(timeUs_t currentTimeUs);
// float getThrottleScale(float globalThrottleScale);
// int16_t getRcCommandOverride(int16_t command[], uint8_t axis);

123
src/main/programming/logic_condition.c
View file

@ -45,8 +45,14 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "io/vtx.h"
#include "drivers/vtx_common.h"
PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 1);
EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags;
EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST];
void pgResetFn_logicConditions(logicCondition_t *instance)
{
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
@ -76,6 +82,8 @@ static int logicConditionCompute(
int operandB
) {
int temporaryValue;
vtxDeviceCapability_t vtxDeviceCapability;
switch (operation) {
case LOGIC_CONDITION_TRUE:
@ -181,6 +189,87 @@ static int logicConditionCompute(
}
break;
case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
return true;
break;
case LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE:
logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE);
return true;
break;
case LOGIC_CONDITION_SWAP_ROLL_YAW:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW);
return true;
break;
case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
if (
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
) {
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
} else {
return false;
}
break;
case LOGIC_CONDITION_SET_VTX_BAND:
if (
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA &&
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
) {
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
} else {
return false;
}
break;
case LOGIC_CONDITION_SET_VTX_CHANNEL:
if (
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA &&
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
) {
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
} else {
return false;
}
break;
case LOGIC_CONDITION_INVERT_ROLL:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
return true;
break;
case LOGIC_CONDITION_INVERT_PITCH:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
return true;
break;
case LOGIC_CONDITION_INVERT_YAW:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
return true;
break;
case LOGIC_CONDITION_OVERRIDE_THROTTLE:
logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
return operandA;
break;
case LOGIC_CONDITION_SET_OSD_LAYOUT:
logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
return operandA;
break;
default:
return false;
break;
@ -453,6 +542,10 @@ int logicConditionGetValue(int8_t conditionId) {
void logicConditionUpdateTask(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
//Disable all flags
logicConditionsGlobalFlags = 0;
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
logicConditionProcess(i);
}
@ -464,3 +557,33 @@ void logicConditionReset(void) {
logicConditionStates[i].flags = 0;
}
}
float NOINLINE getThrottleScale(float globalThrottleScale) {
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE)) {
return constrainf(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] / 100.0f, 0.0f, 1.0f);
} else {
return globalThrottleScale;
}
}
int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
int16_t outputValue = command[axis];
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
outputValue = command[FD_YAW];
} else if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_YAW) {
outputValue = command[FD_ROLL];
}
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
outputValue *= -1;
}
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH) && axis == FD_PITCH) {
outputValue *= -1;
}
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW) && axis == FD_YAW) {
outputValue *= -1;
}
return outputValue;
}

32
src/main/programming/logic_condition.h
View file

@ -50,6 +50,17 @@ typedef enum {
LOGIC_CONDITION_GVAR_SET, // 18
LOGIC_CONDITION_GVAR_INC, // 19
LOGIC_CONDITION_GVAR_DEC, // 20
LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY = 129,
LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE = 130,
LOGIC_CONDITION_SWAP_ROLL_YAW = 131,
LOGIC_CONDITION_SET_VTX_POWER_LEVEL = 132,
LOGIC_CONDITION_INVERT_ROLL = 133,
LOGIC_CONDITION_INVERT_PITCH = 134,
LOGIC_CONDITION_INVERT_YAW = 135,
LOGIC_CONDITION_OVERRIDE_THROTTLE = 136,
LOGIC_CONDITION_SET_VTX_BAND = 137,
LOGIC_CONDITION_SET_VTX_CHANNEL = 138,
LOGIC_CONDITION_SET_OSD_LAYOUT = 139,
LOGIC_CONDITION_LAST
} logicOperation_e;
@ -107,6 +118,17 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,
} logicFlightModeOperands_e;
typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW = (1 << 2),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL = (1 << 3),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
} logicConditionsGlobalFlags_t;
typedef enum {
LOGIC_CONDITION_FLAG_LATCH = 1 << 0,
} logicConditionFlags_e;
@ -132,6 +154,13 @@ typedef struct logicConditionState_s {
uint8_t flags;
} logicConditionState_t;
extern int logicConditionValuesByType[LOGIC_CONDITION_LAST];
extern uint64_t logicConditionsGlobalFlags;
#define LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(mask) (logicConditionsGlobalFlags &= ~(mask))
#define LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(mask) (logicConditionsGlobalFlags |= (mask))
#define LOGIC_CONDITION_GLOBAL_FLAG(mask) (logicConditionsGlobalFlags & (mask))
void logicConditionProcess(uint8_t i);
int logicConditionGetOperandValue(logicOperandType_e type, int operand);
@ -139,3 +168,6 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand);
int logicConditionGetValue(int8_t conditionId);
void logicConditionUpdateTask(timeUs_t currentTimeUs);
void logicConditionReset(void);
float getThrottleScale(float globalThrottleScale);
int16_t getRcCommandOverride(int16_t command[], uint8_t axis);

1
src/main/programming/programming_task.c
View file

@ -31,5 +31,4 @@ FILE_COMPILE_FOR_SIZE
void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) {
logicConditionUpdateTask(currentTimeUs);
globalFunctionsUpdateTask(currentTimeUs);
}