From aa18ab4afa396f31e6c7fae0f8035f0e3a23d7d6 Mon Sep 17 00:00:00 2001 From: Robert Lacroix Date: Wed, 11 Jul 2018 12:04:11 +0200 Subject: [PATCH] Allow linking of modes This allows modes to be linked, for example to link vtx pit mode to paralyze. Whenever paralyze is activated, vtx pit mode is activated as well. Also the logic to prevent mode changes when enabling paralyze can be removed in favor of making paralyze sticky. Modes can be linked in CLI by providing the mode id as the last parameter of the aux command. For example in order to link vtx pit mode to paralyze, replace the last 0 of the pit mode aux (39) with the mode id of paralyze (45): ``` aux 2 39 2 1700 2100 0 0 ``` becomes ``` aux 2 39 2 1700 2100 0 45 ``` _Legal disclaimer: I am making my contributions/submissions to this project solely in my personal capacity and am not conveying any rights to any intellectual property of any third parties._ --- src/main/common/bitarray.c | 9 +++ src/main/common/bitarray.h | 1 + src/main/fc/fc_core.c | 20 +------ src/main/fc/rc_modes.c | 63 ++++++++++++++------- src/main/fc/rc_modes.h | 2 +- src/main/interface/cli.c | 31 +++++++--- src/test/unit/arming_prevention_unittest.cc | 62 ++++++++++++++++---- 7 files changed, 130 insertions(+), 58 deletions(-) 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