diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c index fb3e82c091..1794e60e51 100644 --- a/src/main/common/bitarray.c +++ b/src/main/common/bitarray.c @@ -47,3 +47,12 @@ void bitArrayXor(void *dest, size_t size, void *op1, void *op2) ((uint8_t*)dest)[i] = ((uint8_t*)op1)[i] ^ ((uint8_t*)op2)[i]; } } + +void bitArrayCopy(void *array, unsigned from, unsigned to) +{ + if (bitArrayGet(array, from)) { + bitArraySet(array, to); + } else { + bitArrayClr(array, to); + } +} diff --git a/src/main/common/bitarray.h b/src/main/common/bitarray.h index 1492985893..13fafe5550 100644 --- a/src/main/common/bitarray.h +++ b/src/main/common/bitarray.h @@ -24,3 +24,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); +void bitArrayCopy(void *array, unsigned from, unsigned to); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index fe92dcb607..653d3e7eeb 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -55,7 +55,6 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/fc_core.h" -#include "fc/fc_dispatch.h" #include "fc/fc_rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" @@ -120,8 +119,6 @@ enum { #define DEBUG_RUNAWAY_TAKEOFF_FALSE 0 #endif -#define PARALYZE_PREVENT_MODE_CHANGES_DELAY_US 100000 // Delay after paralyze mode activates to let other mode changes propagate - #if defined(USE_GPS) || defined(USE_MAG) int16_t magHold; #endif @@ -143,15 +140,6 @@ static timeUs_t runawayTakeoffTriggerUs = 0; static bool runawayTakeoffTemporarilyDisabled = false; #endif -static bool paralyzeModeAllowed = false; - -void preventModeChangesDispatch(dispatchEntry_t *self) { - UNUSED(self); - preventModeChanges(); -} - -static dispatchEntry_t preventModeChangesDispatchEntry = { .dispatch = preventModeChangesDispatch}; - PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0); PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, @@ -265,9 +253,8 @@ void updateArmingStatus(void) } #endif - if (IS_RC_MODE_ACTIVE(BOXPARALYZE) && paralyzeModeAllowed) { + if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) { setArmingDisabled(ARMING_DISABLED_PARALYZE); - dispatchAdd(&preventModeChangesDispatchEntry, PARALYZE_PREVENT_MODE_CHANGES_DELAY_US); } if (!isUsingSticksForArming()) { @@ -306,11 +293,6 @@ void updateArmingStatus(void) warningLedUpdate(); } - - // Check if entering into paralyze mode can be allowed regardless of arming status - if (!IS_RC_MODE_ACTIVE(BOXPARALYZE) && !paralyzeModeAllowed) { - paralyzeModeAllowed = true; - } } void disarm(void) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 5c5699cfaf..510a3244b9 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -40,7 +40,8 @@ #include "rx/rx.h" boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e -static bool modeChangesDisabled = false; + +static bool paralyzeModeEverDisabled = false; PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 1); @@ -55,10 +56,6 @@ void rcModeUpdate(boxBitmask_t *newState) rcModeActivationMask = *newState; } -void preventModeChanges(void) { - modeChangesDisabled = true; -} - bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } @@ -77,17 +74,22 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { channelValue < 900 + (range->endStep * 25)); } +void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask) { + boxId_e mode = mac->modeId; + + 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); +} void updateActivatedModes(void) { boxBitmask_t newMask, andMask; memset(&andMask, 0, sizeof(andMask)); - - if (!modeChangesDisabled) { - memset(&newMask, 0, sizeof(newMask)); - } else { - memcpy(&newMask, &rcModeActivationMask, sizeof(newMask)); - } + memset(&newMask, 0, sizeof(newMask)); // determine which conditions set/clear the mode for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -95,21 +97,44 @@ void updateActivatedModes(void) boxId_e mode = mac->modeId; - if (modeChangesDisabled && mode != BOXBEEPERON) { + // Skip linked macs for now to fully determine target states + boxId_e linkedTo = mac->linkedTo; + if (linkedTo) { continue; } - 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) + // Ensure sticky modes are sticky + if (mode == BOXPARALYZE) { + if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) { + bitArrayClr(&andMask, mode); bitArraySet(&newMask, mode); + } else { + if (paralyzeModeEverDisabled) { + updateMasksForMac(mac, &andMask, &newMask); + } else { + paralyzeModeEverDisabled = !isRangeActive(mac->auxChannelIndex, &mac->range); + } + } + } else if (mode < CHECKBOX_ITEM_COUNT) { + updateMasksForMac(mac, &andMask, &newMask); } } + bitArrayXor(&newMask, sizeof(&newMask), &newMask, &andMask); + // Update linked modes + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + const modeActivationCondition_t *mac = modeActivationConditions(i); + + boxId_e linkedTo = mac->linkedTo; + if (!linkedTo) { + continue; + } + + boxId_e mode = mac->modeId; + bitArrayCopy(&newMask, linkedTo, mode); + } + rcModeUpdate(&newMask); } @@ -118,7 +143,7 @@ bool isModeActivationConditionPresent(boxId_e modeId) for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = modeActivationConditions(i); - if (mac->modeId == modeId && IS_RANGE_USABLE(&mac->range)) { + if (mac->modeId == modeId && (IS_RANGE_USABLE(&mac->range) || mac->linkedTo)) { return true; } } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 24a6852630..dda9c33140 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -117,6 +117,7 @@ typedef struct modeActivationCondition_s { uint8_t auxChannelIndex; channelRange_t range; modeLogic_e modeLogic; + boxId_e linkedTo; } modeActivationCondition_t; PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); @@ -129,7 +130,6 @@ typedef struct modeActivationProfile_s { bool IS_RC_MODE_ACTIVE(boxId_e boxId); void rcModeUpdate(boxBitmask_t *newState); -void preventModeChanges(void); bool isAirmodeActive(void); bool isAntiGravityModeActive(void); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index eb881d9be8..bb1ada1e83 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -848,7 +848,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 %u"; + const char *format = "aux %u %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]; @@ -859,8 +859,10 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi && mac->auxChannelIndex == macDefault->auxChannelIndex && mac->range.startStep == macDefault->range.startStep && mac->range.endStep == macDefault->range.endStep - && mac->modeLogic == macDefault->modeLogic; + && mac->modeLogic == macDefault->modeLogic + && mac->linkedTo == macDefault->linkedTo; const box_t *box = findBoxByBoxId(macDefault->modeId); + const box_t *linkedTo = findBoxByBoxId(macDefault->linkedTo); if (box) { cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, @@ -868,11 +870,13 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi macDefault->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep), MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep), - macDefault->modeLogic + macDefault->modeLogic, + linkedTo ? linkedTo->permanentId : 0 ); } } const box_t *box = findBoxByBoxId(mac->modeId); + const box_t *linkedTo = findBoxByBoxId(mac->linkedTo); if (box) { cliDumpPrintLinef(dumpMask, equalsDefault, format, i, @@ -880,7 +884,8 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi mac->auxChannelIndex, MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep), - mac->modeLogic + mac->modeLogic, + linkedTo ? linkedTo->permanentId : 0 ); } } @@ -925,18 +930,30 @@ static void cliAux(char *cmdline) validArgumentCount++; } } + ptr = nextArg(ptr); + if (ptr) { + val = atoi(ptr); + const box_t *box = findBoxByPermanentId(val); + if (box) { + mac->linkedTo = box->boxId; + validArgumentCount++; + } + } if (validArgumentCount == 4) { // for backwards compatibility mac->modeLogic = MODELOGIC_OR; - } else if (validArgumentCount != 5) { + } else if (validArgumentCount == 5) { // for backwards compatibility + mac->linkedTo = 0; + } else if (validArgumentCount != 6) { memset(mac, 0, sizeof(modeActivationCondition_t)); } - cliPrintLinef( "aux %u %u %u %u %u %u", + cliPrintLinef( "aux %u %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 + mac->modeLogic, + mac->linkedTo ); } else { cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 86e1ea2aa9..78d9a1e2f1 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -612,6 +612,50 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, ParalyzeOnAtBoot) +{ + // given + simulationFeatureFlags = 0; + simulationTime = 30e6; // 30 seconds after boot + gyroCalibDone = true; + + // and + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXARM; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOXPARALYZE; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + useRcControlsConfig(NULL); + + // and + rxConfigMutable()->mincheck = 1050; + + // given + rcData[THROTTLE] = 1000; + rcData[AUX1] = 1000; + rcData[AUX2] = 1800; // Paralyze on at boot + ENABLE_STATE(SMALL_ANGLE); + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); + + // when + updateActivatedModes(); + + // expect + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); +} + TEST(ArmingPreventionTest, Paralyze) { // given @@ -632,10 +676,8 @@ TEST(ArmingPreventionTest, Paralyze) modeActivationConditionsMutable(2)->modeId = BOXBEEPERON; modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); - modeActivationConditionsMutable(3)->auxChannelIndex = 3; modeActivationConditionsMutable(3)->modeId = BOXVTXPITMODE; - modeActivationConditionsMutable(3)->range.startStep = CHANNEL_VALUE_TO_STEP(1750); - modeActivationConditionsMutable(3)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + modeActivationConditionsMutable(3)->linkedTo = BOXPARALYZE; useRcControlsConfig(NULL); // and @@ -646,7 +688,6 @@ TEST(ArmingPreventionTest, Paralyze) rcData[AUX1] = 1000; rcData[AUX2] = 1000; rcData[AUX3] = 1000; - rcData[AUX4] = 1000; ENABLE_STATE(SMALL_ANGLE); // when @@ -687,9 +728,8 @@ TEST(ArmingPreventionTest, Paralyze) EXPECT_EQ(0, getArmingDisableFlags()); // given - // paraylze and enter pit mode + // paralyze (and activate linked vtx pit mode) rcData[AUX2] = 1800; - rcData[AUX4] = 1800; // when updateActivatedModes(); @@ -701,12 +741,8 @@ TEST(ArmingPreventionTest, Paralyze) EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE)); EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXBEEPERON)); - // and - preventModeChanges(); - // given - // Try exiting pit mode and enable beeper - rcData[AUX4] = 1000; + // enable beeper rcData[AUX3] = 1800; // when @@ -717,7 +753,7 @@ TEST(ArmingPreventionTest, Paralyze) EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXBEEPERON)); // given - // exit paralyze mode and ensure arming is still disabled + // try exiting paralyze mode and ensure arming and pit mode are still disabled rcData[AUX2] = 1000; // when @@ -727,6 +763,8 @@ TEST(ArmingPreventionTest, Paralyze) // expect EXPECT_TRUE(isArmingDisabled()); EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags()); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE)); } // STUBS