diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c index cfa1ed0ae1..e767d2eea2 100644 --- a/src/main/common/logic_condition.c +++ b/src/main/common/logic_condition.c @@ -44,7 +44,27 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" -PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 1); + +void pgResetFn_logicConditions(logicCondition_t *instance) +{ + for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { + RESET_CONFIG(logicCondition_t, &instance[i], + .enabled = 0, + .activatorId = -1, + .operation = 0, + .operandA = { + .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE, + .value = 0 + }, + .operandB = { + .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE, + .value = 0 + }, + .flags = 0 + ); + } +} logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS]; diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h index 3dc3e7e77c..a158875437 100644 --- a/src/main/common/logic_condition.h +++ b/src/main/common/logic_condition.h @@ -115,6 +115,7 @@ typedef struct logicOperand_s { typedef struct logicCondition_s { uint8_t enabled; + int8_t activatorId; logicOperation_e operation; logicOperand_t operandA; logicOperand_t operandB; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 107494b54a..427df2bbb1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1758,7 +1758,7 @@ static void cliServoMix(char *cmdline) static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions) { - const char *format = "logic %d %d %d %d %d %d %d %d"; + const char *format = "logic %d %d %d %d %d %d %d %d %d"; for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { const logicCondition_t logic = logicConditions[i]; @@ -1767,6 +1767,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions logicCondition_t defaultValue = defaultLogicConditions[i]; equalsDefault = logic.enabled == defaultValue.enabled && + logic.activatorId == defaultValue.activatorId && logic.operation == defaultValue.operation && logic.operandA.type == defaultValue.operandA.type && logic.operandA.value == defaultValue.operandA.value && @@ -1777,6 +1778,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, logic.enabled, + logic.activatorId, logic.operation, logic.operandA.type, logic.operandA.value, @@ -1788,6 +1790,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions cliDumpPrintLinef(dumpMask, equalsDefault, format, i, logic.enabled, + logic.activatorId, logic.operation, logic.operandA.type, logic.operandA.value, @@ -1800,7 +1803,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions static void cliLogic(char *cmdline) { char * saveptr; - int args[8], check = 0; + int args[9], check = 0; uint8_t len = strlen(cmdline); if (len == 0) { @@ -1811,6 +1814,7 @@ static void cliLogic(char *cmdline) { enum { INDEX = 0, ENABLED, + ACTIVATOR_ID, OPERATION, OPERAND_A_TYPE, OPERAND_A_VALUE, @@ -1834,6 +1838,7 @@ static void cliLogic(char *cmdline) { if ( i >= 0 && i < MAX_LOGIC_CONDITIONS && args[ENABLED] >= 0 && args[ENABLED] <= 1 && + args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS && args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST && args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST && args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 && @@ -1843,6 +1848,7 @@ static void cliLogic(char *cmdline) { ) { logicConditionsMutable(i)->enabled = args[ENABLED]; + logicConditionsMutable(i)->activatorId = args[ACTIVATOR_ID]; logicConditionsMutable(i)->operation = args[OPERATION]; logicConditionsMutable(i)->operandA.type = args[OPERAND_A_TYPE]; logicConditionsMutable(i)->operandA.value = args[OPERAND_A_VALUE]; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0e890efb1e..6dea4773db 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -530,6 +530,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_LOGIC_CONDITIONS: for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { sbufWriteU8(dst, logicConditions(i)->enabled); + sbufWriteU8(dst, logicConditions(i)->activatorId); sbufWriteU8(dst, logicConditions(i)->operation); sbufWriteU8(dst, logicConditions(i)->operandA.type); sbufWriteU32(dst, logicConditions(i)->operandA.value); @@ -1949,8 +1950,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_LOGIC_CONDITIONS case MSP2_INAV_SET_LOGIC_CONDITIONS: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize == 14) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) { + if ((dataSize == 15) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) { logicConditionsMutable(tmp_u8)->enabled = sbufReadU8(src); + logicConditionsMutable(tmp_u8)->activatorId = sbufReadU8(src); logicConditionsMutable(tmp_u8)->operation = sbufReadU8(src); logicConditionsMutable(tmp_u8)->operandA.type = sbufReadU8(src); logicConditionsMutable(tmp_u8)->operandA.value = sbufReadU32(src);