diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b9b35d810d..6432dd4140 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -54,6 +54,7 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicCon EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags; EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST]; +EXTENDED_FASTRAM rcChannelOverride_t rcChannelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT]; void pgResetFn_logicConditions(logicCondition_t *instance) { @@ -307,6 +308,14 @@ static int logicConditionCompute( return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB); break; + case LOGIC_CONDITION_RC_CHANNEL_OVERRIDE: + temporaryValue = constrain(operandA - 1, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1); + rcChannelOverrides[temporaryValue].active = true; + rcChannelOverrides[temporaryValue].value = constrain(operandB, PWM_RANGE_MIN, PWM_RANGE_MAX); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL); + return true; + break; + default: return false; break; @@ -622,6 +631,11 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) { for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { logicConditionProcess(i); } + + for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + rcChannelOverrides[i].active = false; + } + #ifdef USE_I2C_IO_EXPANDER ioPortExpanderSync(); #endif @@ -663,3 +677,11 @@ int16_t getRcCommandOverride(int16_t command[], uint8_t axis) { return outputValue; } + +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) { + if (rcChannelOverrides[channel].active) { + return rcChannelOverrides[channel].value; + } else { + return originalValue; + } +} diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 75905c51ca..8c8167e6ac 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -67,7 +67,8 @@ typedef enum { LOGIC_CONDITION_TAN = 35, LOGIC_CONDITION_MAP_INPUT = 36, LOGIC_CONDITION_MAP_OUTPUT = 37, - LOGIC_CONDITION_LAST = 38, + LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, + LOGIC_CONDITION_LAST = 39, } logicOperation_e; typedef enum logicOperandType_s { @@ -141,6 +142,7 @@ typedef enum { 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), + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), } logicConditionsGlobalFlags_t; typedef enum { @@ -168,6 +170,11 @@ typedef struct logicConditionState_s { uint8_t flags; } logicConditionState_t; +typedef struct rcChannelOverride_s { + uint8_t active; + int value; +} rcChannelOverride_t; + extern int logicConditionValuesByType[LOGIC_CONDITION_LAST]; extern uint64_t logicConditionsGlobalFlags; @@ -184,4 +191,5 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs); void logicConditionReset(void); float getThrottleScale(float globalThrottleScale); -int16_t getRcCommandOverride(int16_t command[], uint8_t axis); \ No newline at end of file +int16_t getRcCommandOverride(int16_t command[], uint8_t axis); +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); \ No newline at end of file diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index fa24d6ceb9..1df4ccfac6 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -29,6 +29,8 @@ #include "common/maths.h" #include "common/utils.h" +#include "programming/logic_condition.h" + #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -713,7 +715,11 @@ uint16_t rxGetRefreshRate(void) int16_t rxGetChannelValue(unsigned channelNumber) { - return rcChannels[channelNumber].data; + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL)) { + return getRcChannelOverride(channelNumber, rcChannels[channelNumber].data); + } else { + return rcChannels[channelNumber].data; + } } int16_t rxGetRawChannelValue(unsigned channelNumber)