diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 510a3244b..258a1abbd 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -28,6 +28,7 @@ #include "common/bitarray.h" #include "common/maths.h" +#include "drivers/time.h" #include "config/feature.h" #include "pg/pg.h" @@ -39,9 +40,10 @@ #include "rx/rx.h" -boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e +#define STICKY_MODE_BOOT_DELAY_US 5e6 -static bool paralyzeModeEverDisabled = false; +boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e +static boxBitmask_t stickyModesEverDisabled; PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 1); @@ -74,48 +76,52 @@ 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); +void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask) +{ + bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(andMask, mac->modeId); bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range); if (bAnd) - bitArraySet(andMask, mode); + bitArraySet(andMask, mac->modeId); if (bAnd != bAct) - bitArraySet(newMask, mode); + bitArraySet(newMask, mac->modeId); +} + +void updateMasksForStickyModes(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask) +{ + if (IS_RC_MODE_ACTIVE(mac->modeId)) { + bitArrayClr(andMask, mac->modeId); + bitArraySet(newMask, mac->modeId); + } else { + if (bitArrayGet(&stickyModesEverDisabled, mac->modeId)) { + updateMasksForMac(mac, andMask, newMask); + } else { + if (micros() >= STICKY_MODE_BOOT_DELAY_US && !isRangeActive(mac->auxChannelIndex, &mac->range)) { + bitArraySet(&stickyModesEverDisabled, mac->modeId); + } + } + } } void updateActivatedModes(void) { - boxBitmask_t newMask, andMask; + boxBitmask_t newMask, andMask, stickyModes; memset(&andMask, 0, sizeof(andMask)); memset(&newMask, 0, sizeof(newMask)); + memset(&stickyModes, 0, sizeof(stickyModes)); + bitArraySet(&stickyModes, BOXPARALYZE); // determine which conditions set/clear the mode for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = modeActivationConditions(i); - boxId_e mode = mac->modeId; - // Skip linked macs for now to fully determine target states - boxId_e linkedTo = mac->linkedTo; - if (linkedTo) { + if (mac->linkedTo) { continue; } - // 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) { + if (bitArrayGet(&stickyModes, mac->modeId)) { + updateMasksForStickyModes(mac, &andMask, &newMask); + } else if (mac->modeId < CHECKBOX_ITEM_COUNT) { updateMasksForMac(mac, &andMask, &newMask); } } @@ -126,13 +132,11 @@ void updateActivatedModes(void) for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = modeActivationConditions(i); - boxId_e linkedTo = mac->linkedTo; - if (!linkedTo) { + if (!mac->linkedTo) { continue; } - boxId_e mode = mac->modeId; - bitArrayCopy(&newMask, linkedTo, mode); + bitArrayCopy(&newMask, mac->linkedTo, mac->modeId); } rcModeUpdate(&newMask); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 78d9a1e2f..3b96f8972 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -616,7 +616,7 @@ TEST(ArmingPreventionTest, ParalyzeOnAtBoot) { // given simulationFeatureFlags = 0; - simulationTime = 30e6; // 30 seconds after boot + simulationTime = 0; gyroCalibDone = true; // and @@ -660,7 +660,7 @@ TEST(ArmingPreventionTest, Paralyze) { // given simulationFeatureFlags = 0; - simulationTime = 30e6; // 30 seconds after boot + simulationTime = 0; gyroCalibDone = true; // and @@ -686,7 +686,7 @@ TEST(ArmingPreventionTest, Paralyze) // given rcData[THROTTLE] = 1000; rcData[AUX1] = 1000; - rcData[AUX2] = 1000; + rcData[AUX2] = 1800; // Start out with paralyze enabled rcData[AUX3] = 1000; ENABLE_STATE(SMALL_ANGLE); @@ -726,9 +726,28 @@ TEST(ArmingPreventionTest, Paralyze) EXPECT_FALSE(ARMING_FLAG(ARMED)); EXPECT_FALSE(isArmingDisabled()); EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); // given - // paralyze (and activate linked vtx pit mode) + simulationTime = 10e6; // 10 seconds after boot + + // when + updateActivatedModes(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE)); + + // given + // disable paralyze once after the startup timer + rcData[AUX2] = 1000; + + // when + updateActivatedModes(); + + // enable paralyze again rcData[AUX2] = 1800; // when @@ -738,6 +757,7 @@ 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)); EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXBEEPERON)); diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index d40539e4f..4fe3e4aea 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -555,6 +555,11 @@ uint32_t millis(void) return sysTickUptime; } +uint32_t micros(void) +{ + return millis() * 1000; +} + throttleStatus_e calculateThrottleStatus() { return throttleStatus; diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index bf20ccaf2..d8b67449b 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -224,6 +224,10 @@ extern "C" { uint32_t millis(void) { return fixedMillis; } + +uint32_t micros(void) { + return fixedMillis * 1000; +} } void resetMillis(void) { diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index 00a1bdf78..a255e4d83 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -888,6 +888,7 @@ extern "C" { } uint32_t millis(void) { return testData.millis++; } + uint32_t micros(void) { return millis() * 1000; } void beeper(beeperMode_e mode) { UNUSED(mode); } uint8_t armingFlags = 0; bool cmsInMenu;