diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c index 41a5b48761..a4ac13ec7a 100644 --- a/src/main/common/bitarray.c +++ b/src/main/common/bitarray.c @@ -17,6 +17,7 @@ #include #include +#include #include "bitarray.h" @@ -36,3 +37,10 @@ void bitArrayClr(void *array, unsigned bit) { BITARRAY_BIT_OP((uint32_t*)array, bit, &=~); } + +void bitArrayXor(void *dest, size_t size, void *op1, void *op2) +{ + for (size_t i = 0; i < size; i++) { + ((uint8_t*)dest)[i] = ((uint8_t*)op1)[i] ^ ((uint8_t*)op2)[i]; + } +} diff --git a/src/main/common/bitarray.h b/src/main/common/bitarray.h index 50dab5909a..6dc185c74a 100644 --- a/src/main/common/bitarray.h +++ b/src/main/common/bitarray.h @@ -20,3 +20,4 @@ bool bitArrayGet(const void *array, unsigned bit); void bitArraySet(void *array, unsigned bit); void bitArrayClr(void *array, unsigned bit); +void bitArrayXor(void *dest, size_t size, void *op1, void *op2); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 161c39db14..dffc8e4cd0 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -40,7 +40,6 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); - bool IS_RC_MODE_ACTIVE(boxId_e boxId) { return bitArrayGet(&rcModeActivationMask, boxId); @@ -72,29 +71,35 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { void updateActivatedModes(void) { - boxBitmask_t newMask; + boxBitmask_t newMask, andMask; memset(&newMask, 0, sizeof(newMask)); + memset(&andMask, 0, sizeof(andMask)); - for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); + // determine which conditions set/clear the mode + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + const modeActivationCondition_t *mac = modeActivationConditions(i); - if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) { - boxId_e mode = modeActivationCondition->modeId; - if (mode < CHECKBOX_ITEM_COUNT) + boxId_e mode = mac->modeId; + if (mode < CHECKBOX_ITEM_COUNT) { + bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(&andMask, mode); + bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range); + if (bAnd) + bitArraySet(&andMask, mode); + if (bAnd != bAct) bitArraySet(&newMask, mode); } } + bitArrayXor(&newMask, sizeof(&newMask), &newMask, &andMask); + rcModeUpdate(&newMask); } bool isModeActivationConditionPresent(boxId_e modeId) { - uint8_t index; + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + const modeActivationCondition_t *mac = modeActivationConditions(i); - for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); - - if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { + if (mac->modeId == modeId && IS_RANGE_USABLE(&mac->range)) { return true; } } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 8c1d957826..ec217e7aa1 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -65,6 +65,11 @@ typedef enum { CHECKBOX_ITEM_COUNT } boxId_e; +typedef enum { + MODELOGIC_OR = 0, + MODELOGIC_AND +} modeLogic_e; + // type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior typedef struct boxBitmask_s { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t; @@ -82,7 +87,7 @@ typedef struct boxBitmask_s { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } // steps are 25 apart // a value of 0 corresponds to a channel value of 900 or less // a value of 48 corresponds to a channel value of 2100 or more -// 48 steps between 900 and 1200 +// 48 steps between 900 and 2100 typedef struct channelRange_s { uint8_t startStep; uint8_t endStep; @@ -92,6 +97,7 @@ typedef struct modeActivationCondition_s { boxId_e modeId; uint8_t auxChannelIndex; channelRange_t range; + modeLogic_e modeLogic; } modeActivationCondition_t; PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 9caca64345..5d6c0a0f39 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -689,7 +689,7 @@ static void cliRxFailsafe(char *cmdline) static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions) { - const char *format = "aux %u %u %u %u %u"; + const char *format = "aux %u %u %u %u %u %u"; // print out aux channel settings for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = &modeActivationConditions[i]; @@ -699,7 +699,8 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi equalsDefault = mac->modeId == macDefault->modeId && mac->auxChannelIndex == macDefault->auxChannelIndex && mac->range.startStep == macDefault->range.startStep - && mac->range.endStep == macDefault->range.endStep; + && mac->range.endStep == macDefault->range.endStep + && mac->modeLogic == macDefault->modeLogic; const box_t *box = findBoxByBoxId(macDefault->modeId); if (box) { cliDefaultPrintLinef(dumpMask, equalsDefault, format, @@ -707,7 +708,8 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi box->permanentId, macDefault->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep) + MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep), + macDefault->modeLogic ); } } @@ -718,7 +720,8 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi box->permanentId, mac->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) + MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep), + mac->modeLogic ); } } @@ -755,10 +758,27 @@ static void cliAux(char *cmdline) } } ptr = processChannelRangeArgs(ptr, &mac->range, &validArgumentCount); - - if (validArgumentCount != 4) { + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + if (val == MODELOGIC_OR || val == MODELOGIC_AND) { + mac->modeLogic = val; + validArgumentCount++; + } + } + if (validArgumentCount == 4) { // for backwards compatibility + mac->modeLogic = MODELOGIC_OR; + } else if (validArgumentCount != 5) { memset(mac, 0, sizeof(modeActivationCondition_t)); } + cliPrintLinef( "aux %u %u %u %u %u %u", + i, + mac->modeId, + mac->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep), + mac->modeLogic + ); } else { cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1); } @@ -3622,7 +3642,7 @@ static void cliHelp(char *cmdline); // should be sorted a..z for bsearch() const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange), - CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), + CLI_COMMAND_DEF("aux", "configure modes", " ", cliAux), #ifdef BEEPER CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper),