diff --git a/.travis.sh b/.travis.sh index 865918d69e..7332d8aaaf 100755 --- a/.travis.sh +++ b/.travis.sh @@ -10,7 +10,7 @@ TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} -MAKE="make EXTRA_FLAGS=-Werror V=0" +MAKE="make EXTRA_FLAGS=-Werror" CURL_BASEOPTS=( "--retry" "10" diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 43912d31b0..c793fa9eac 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -56,6 +56,7 @@ #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,6 +121,7 @@ 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; @@ -140,6 +142,14 @@ 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); @@ -244,6 +254,11 @@ void updateArmingStatus(void) } } + if (IS_RC_MODE_ACTIVE(BOXPARALYZE) && paralyzeModeAllowed) { + setArmingDisabled(ARMING_DISABLED_PARALYZE); + dispatchAdd(&preventModeChangesDispatchEntry, PARALYZE_PREVENT_MODE_CHANGES_DELAY_US); + } + if (!isUsingSticksForArming()) { /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */ bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm @@ -280,6 +295,11 @@ 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 432df5d90f..42077533fa 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -39,6 +39,7 @@ #include "rx/rx.h" boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e +static bool modeChangesDisabled = false; PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); @@ -53,6 +54,10 @@ void rcModeUpdate(boxBitmask_t *newState) rcModeActivationMask = *newState; } +void preventModeChanges(void) { + modeChangesDisabled = true; +} + bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } @@ -75,14 +80,24 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { void updateActivatedModes(void) { boxBitmask_t newMask, andMask; - memset(&newMask, 0, sizeof(newMask)); memset(&andMask, 0, sizeof(andMask)); + if (!modeChangesDisabled) { + memset(&newMask, 0, sizeof(newMask)); + } else { + memcpy(&newMask, &rcModeActivationMask, sizeof(newMask)); + } + // 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; + + if (modeChangesDisabled && mode != BOXBEEPERON) { + continue; + } + if (mode < CHECKBOX_ITEM_COUNT) { bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(&andMask, mode); bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index fa4935d8d1..415cc269d2 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -71,6 +71,7 @@ typedef enum { BOXPREARM, BOXBEEPGPSCOUNT, BOXVTXPITMODE, + BOXPARALYZE, BOXUSER1, BOXUSER2, BOXUSER3, @@ -124,6 +125,7 @@ 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/fc/runtime_config.c b/src/main/fc/runtime_config.c index 016692d3c9..7a007193e7 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -51,6 +51,7 @@ const char *armingDisableFlagNames[]= { "OSD", "BST", "MSP", + "PARALYZE", "ARMSWITCH" }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 0452539a26..b49a75b393 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -55,10 +55,11 @@ typedef enum { ARMING_DISABLED_OSD_MENU = (1 << 14), ARMING_DISABLED_BST = (1 << 15), ARMING_DISABLED_MSP = (1 << 16), - ARMING_DISABLED_ARM_SWITCH = (1 << 17), // Needs to be the last element, since it's always activated if one of the others is active when arming + ARMING_DISABLED_PARALYZE = (1 << 17), + ARMING_DISABLED_ARM_SWITCH = (1 << 18), // Needs to be the last element, since it's always activated if one of the others is active when arming } armingDisableFlags_e; -#define ARMING_DISABLE_FLAGS_COUNT 18 +#define ARMING_DISABLE_FLAGS_COUNT 19 extern const char *armingDisableFlagNames[ARMING_DISABLE_FLAGS_COUNT]; diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index 25cbfc4df0..a75b0ec7f1 100644 --- a/src/main/interface/msp_box.c +++ b/src/main/interface/msp_box.c @@ -92,6 +92,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXUSER3, "USER3", 42 }, { BOXUSER4, "USER4", 43 }, { BOXPIDAUDIO, "PID AUDIO", 44 }, + { BOXPARALYZE, "PARALYZE", 45 }, }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index @@ -269,6 +270,8 @@ void initActiveBoxIds(void) BME(BOXVTXPITMODE); #endif + BME(BOXPARALYZE); + #ifdef USE_PINIOBOX // Turn BOXUSERx only if pinioBox facility monitors them, as the facility is the only BOXUSERx observer. // Note that pinioBoxConfig can be set to monitor any box. diff --git a/src/test/Makefile b/src/test/Makefile index a1c34c0188..346811727b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -28,6 +28,7 @@ alignsensor_unittest_SRC := \ arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/fc_core.c \ + $(USER_DIR)/fc/fc_dispatch.c \ $(USER_DIR)/fc/rc_controls.c \ $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/runtime_config.c \ diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 786a904544..771d84ebc8 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -609,6 +609,123 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit EXPECT_EQ(0, getArmingDisableFlags()); } +TEST(ArmingPreventionTest, Paralyze) +{ + // 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); + modeActivationConditionsMutable(2)->auxChannelIndex = 2; + 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); + useRcControlsConfig(NULL); + + // and + rxConfigMutable()->mincheck = 1050; + + // given + rcData[THROTTLE] = 1000; + rcData[AUX1] = 1000; + rcData[AUX2] = 1000; + rcData[AUX3] = 1000; + rcData[AUX4] = 1000; + ENABLE_STATE(SMALL_ANGLE); + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + + // given + // arm + rcData[AUX1] = 1800; + + // when + tryArm(); + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_TRUE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + + // given + // disarm + rcData[AUX1] = 1000; + + // when + disarm(); + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_FALSE(ARMING_FLAG(ARMED)); + EXPECT_FALSE(isArmingDisabled()); + EXPECT_EQ(0, getArmingDisableFlags()); + + // given + // paraylze and enter pit mode + rcData[AUX2] = 1800; + rcData[AUX4] = 1800; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags()); + 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; + rcData[AUX3] = 1800; + + // when + updateActivatedModes(); + + // expect + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXBEEPERON)); + + // given + // exit paralyze mode and ensure arming is still disabled + rcData[AUX2] = 1000; + + // when + updateActivatedModes(); + updateArmingStatus(); + + // expect + EXPECT_TRUE(isArmingDisabled()); + EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags()); +} + // STUBS extern "C" { uint32_t micros(void) { return simulationTime; }