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

@@ -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"

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)

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);

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);

src/main/fc/runtime_config.c

@@ -51,6 +51,7 @@ const char *armingDisableFlagNames[]= {
     "OSD",
     "BST",
     "MSP",
+    "PARALYZE",
     "ARMSWITCH"
 };
 

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];
 

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.

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 \

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; }