From 092baf5805c3cd5dc3567dddcc704a034785a839 Mon Sep 17 00:00:00 2001 From: Robert Lacroix Date: Mon, 7 May 2018 21:41:57 -0700 Subject: [PATCH] Add Paralyze mode support During team relay races it's unsafe to retrieve crashed quads because the course is continuously hot. In order to safely fly a backup quad with the primary quad crashed in the field (but powered up) it's necessary to: * Disable arming, so that the crashed quad doesn't unintentionally arm as well. This is specifically a problem when a transmitter can send signals to all powered up receivers (like FrSky and others). * Change VTX to an unused channel with low power output * Turn off telemetry This change introduces a new mode called paralyze which disables arming and prevents mode changes (except beeper). It can only be invoked while the quad isn't armed. Once it's invoked, the FC has to be power cycled. In order to invoke it, the mode needs to be in a disengaged state at least once, so that forgetting to flip the switch back after crashing doesn't immediately invoke graveyard on the backup quad. _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._ --- .travis.sh | 2 +- src/main/fc/fc_core.c | 20 ++++ src/main/fc/rc_modes.c | 17 ++- src/main/fc/rc_modes.h | 2 + src/main/fc/runtime_config.c | 1 + src/main/fc/runtime_config.h | 5 +- src/main/interface/msp_box.c | 3 + src/test/Makefile | 1 + src/test/unit/arming_prevention_unittest.cc | 117 ++++++++++++++++++++ 9 files changed, 164 insertions(+), 4 deletions(-) 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 d4304b6742..1b0cdb2164 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" @@ -121,6 +122,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; @@ -141,6 +143,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); @@ -245,6 +255,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 @@ -281,6 +296,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; }