Merge pull request #5851 from robertlacroix/graveyard

Add Paralyze mode support

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

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