Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-16 04:45:22 +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 ( if (
!isArmingDisabled() || !isArmingDisabled() ||
emergencyArmingIsEnabled() || emergencyArmingIsEnabled() ||
GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY) LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)
) { ) {
#else #else
if ( 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. // Find min and max throttle based on condition.
#ifdef USE_PROGRAMMING_FRAMEWORK #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; throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = motorConfig()->maxthrottle;
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
} else } else
#endif #endif
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {

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

@ -3243,8 +3243,8 @@ void osdUpdate(timeUs_t currentTimeUs)
activeLayout = 1; activeLayout = 1;
else else
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT)) if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT))
activeLayout = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT); activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT);
else else
#endif #endif
activeLayout = 0; 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); PG_REGISTER_ARRAY_WITH_RESET_FN(globalFunction_t, MAX_GLOBAL_FUNCTIONS, globalFunctions, PG_GLOBAL_FUNCTIONS, 0);
EXTENDED_FASTRAM uint64_t globalFunctionsFlags = 0; // EXTENDED_FASTRAM uint64_t globalFunctionsFlags = 0;
EXTENDED_FASTRAM globalFunctionState_t globalFunctionsStates[MAX_GLOBAL_FUNCTIONS]; // EXTENDED_FASTRAM globalFunctionState_t globalFunctionsStates[MAX_GLOBAL_FUNCTIONS];
EXTENDED_FASTRAM int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST]; // EXTENDED_FASTRAM int globalFunctionValues[GLOBAL_FUNCTION_ACTION_LAST];
void pgResetFn_globalFunctions(globalFunction_t *instance) void pgResetFn_globalFunctions(globalFunction_t *instance)
{ {
@ -64,126 +64,118 @@ void globalFunctionsProcess(int8_t functionId) {
//Process only activated functions //Process only activated functions
if (globalFunctions(functionId)->enabled) { if (globalFunctions(functionId)->enabled) {
const int conditionValue = logicConditionGetValue(globalFunctions(functionId)->conditionId); // const int conditionValue = logicConditionGetValue(globalFunctions(functionId)->conditionId);
const int previousValue = globalFunctionsStates[functionId].active; // const int previousValue = globalFunctionsStates[functionId].active;
globalFunctionsStates[functionId].active = (bool) conditionValue; // globalFunctionsStates[functionId].active = (bool) conditionValue;
globalFunctionsStates[functionId].value = logicConditionGetOperandValue( // globalFunctionsStates[functionId].value = logicConditionGetOperandValue(
globalFunctions(functionId)->withValue.type, // globalFunctions(functionId)->withValue.type,
globalFunctions(functionId)->withValue.value // globalFunctions(functionId)->withValue.value
); // );
switch (globalFunctions(functionId)->action) { // switch (globalFunctions(functionId)->action) {
case GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY: // case GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY:
if (conditionValue) { // if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_ARMING_SAFETY); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE: // case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE:
if (conditionValue) { // if (conditionValue) {
globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] = globalFunctionsStates[functionId].value; // globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW: // case GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW:
if (conditionValue) { // if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL: // case GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL:
if (conditionValue && !previousValue) { // if (conditionValue && !previousValue) {
vtxDeviceCapability_t vtxDeviceCapability; // vtxDeviceCapability_t vtxDeviceCapability;
if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { // if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
vtxSettingsConfigMutable()->power = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); // vtxSettingsConfigMutable()->power = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
} // }
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_SET_VTX_BAND: // case GLOBAL_FUNCTION_ACTION_SET_VTX_BAND:
if (conditionValue && !previousValue) { // if (conditionValue && !previousValue) {
vtxDeviceCapability_t vtxDeviceCapability; // vtxDeviceCapability_t vtxDeviceCapability;
if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { // if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
vtxSettingsConfigMutable()->band = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND); // vtxSettingsConfigMutable()->band = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
} // }
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL: // case GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL:
if (conditionValue && !previousValue) { // if (conditionValue && !previousValue) {
vtxDeviceCapability_t vtxDeviceCapability; // vtxDeviceCapability_t vtxDeviceCapability;
if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { // if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
vtxSettingsConfigMutable()->channel = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL); // vtxSettingsConfigMutable()->channel = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
} // }
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_INVERT_ROLL: // case GLOBAL_FUNCTION_ACTION_INVERT_ROLL:
if (conditionValue) { // if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_INVERT_PITCH: // case GLOBAL_FUNCTION_ACTION_INVERT_PITCH:
if (conditionValue) { // if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_INVERT_YAW: // case GLOBAL_FUNCTION_ACTION_INVERT_YAW:
if (conditionValue) { // if (conditionValue) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE: // case GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE:
if (conditionValue) { // if (conditionValue) {
globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].value; // globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
} // }
break; // break;
case GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT: // case GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT:
if(conditionValue){ // if(conditionValue){
globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT] = globalFunctionsStates[functionId].value; // globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT); // LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
} // }
break; // break;
} // }
} }
} }
void NOINLINE globalFunctionsUpdateTask(timeUs_t currentTimeUs) { // void NOINLINE globalFunctionsUpdateTask(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs); // UNUSED(currentTimeUs);
//Disable all flags // //Disable all flags
globalFunctionsFlags = 0; // globalFunctionsFlags = 0;
for (uint8_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) { // for (uint8_t i = 0; i < MAX_GLOBAL_FUNCTIONS; i++) {
globalFunctionsProcess(i); // globalFunctionsProcess(i);
} // }
} // }
float NOINLINE getThrottleScale(float globalThrottleScale) { // int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE_SCALE)) { // int16_t outputValue = command[axis];
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) { // if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
int16_t outputValue = command[axis]; // 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) { // if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
outputValue = command[FD_YAW]; // outputValue *= -1;
} 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_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) { // return outputValue;
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;
}
#endif #endif

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

@ -43,17 +43,6 @@ typedef enum {
GLOBAL_FUNCTION_ACTION_LAST GLOBAL_FUNCTION_ACTION_LAST
} globalFunctionActions_e; } 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 { typedef struct globalFunction_s {
uint8_t enabled; uint8_t enabled;
int8_t conditionId; int8_t conditionId;
@ -70,13 +59,9 @@ typedef struct globalFunctionState_s {
extern uint64_t globalFunctionsFlags; 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); 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); // void globalFunctionsUpdateTask(timeUs_t currentTimeUs);
float getThrottleScale(float globalThrottleScale); // float getThrottleScale(float globalThrottleScale);
int16_t getRcCommandOverride(int16_t command[], uint8_t axis); // 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.h"
#include "navigation/navigation_private.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); 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) void pgResetFn_logicConditions(logicCondition_t *instance)
{ {
for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
@ -76,6 +82,8 @@ static int logicConditionCompute(
int operandB int operandB
) { ) {
int temporaryValue; int temporaryValue;
vtxDeviceCapability_t vtxDeviceCapability;
switch (operation) { switch (operation) {
case LOGIC_CONDITION_TRUE: case LOGIC_CONDITION_TRUE:
@ -181,6 +189,87 @@ static int logicConditionCompute(
} }
break; 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: default:
return false; return false;
break; break;
@ -453,6 +542,10 @@ int logicConditionGetValue(int8_t conditionId) {
void logicConditionUpdateTask(timeUs_t currentTimeUs) { void logicConditionUpdateTask(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
//Disable all flags
logicConditionsGlobalFlags = 0;
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
logicConditionProcess(i); logicConditionProcess(i);
} }
@ -464,3 +557,33 @@ void logicConditionReset(void) {
logicConditionStates[i].flags = 0; 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_SET, // 18
LOGIC_CONDITION_GVAR_INC, // 19 LOGIC_CONDITION_GVAR_INC, // 19
LOGIC_CONDITION_GVAR_DEC, // 20 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 LOGIC_CONDITION_LAST
} logicOperation_e; } logicOperation_e;
@ -107,6 +118,17 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR, LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,
} logicFlightModeOperands_e; } 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 { typedef enum {
LOGIC_CONDITION_FLAG_LATCH = 1 << 0, LOGIC_CONDITION_FLAG_LATCH = 1 << 0,
} logicConditionFlags_e; } logicConditionFlags_e;
@ -132,6 +154,13 @@ typedef struct logicConditionState_s {
uint8_t flags; uint8_t flags;
} logicConditionState_t; } 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); void logicConditionProcess(uint8_t i);
int logicConditionGetOperandValue(logicOperandType_e type, int operand); int logicConditionGetOperandValue(logicOperandType_e type, int operand);
@ -139,3 +168,6 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand);
int logicConditionGetValue(int8_t conditionId); int logicConditionGetValue(int8_t conditionId);
void logicConditionUpdateTask(timeUs_t currentTimeUs); void logicConditionUpdateTask(timeUs_t currentTimeUs);
void logicConditionReset(void); 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) { void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) {
logicConditionUpdateTask(currentTimeUs); logicConditionUpdateTask(currentTimeUs);
globalFunctionsUpdateTask(currentTimeUs);
} }