Update turtle / crashflip mode (#13905)

* stop motors after 90 degrees of rotation and with max rate

* handle no accelerometer data

* improve check for acc, although seems to be OK without it

* disable all attenuation if rate is set to zero

* refactoring thanks Karate

* use sensors.h

* remove unnecessary arming check

* exit crashFlip immediately switch is reverted if throttle is zero

* add Crashflip Rate to OSD

* Revert unnecessary changes in crashflip core.c code

and clarify comments about crashflip switch

* update / minimise comments, thanks Karate

* ensure all names say `crashflip` consistently

* Undo quick re-arm because motrors were not reversed

* fix issue with reversed motors, we must disarm

* ignore yaw rotation and set gyro limit to 1900 deg/s

* default attenuation to off (crashflip_rate = 0)

* refactoring, increase rate limit to allow stronger inhibition

* enable in race_pro mode

* don't attenuate on attitude until a significant change occurs

* no attenuation for small changes

* Updates from review by PL

* remove whitespace

* refactor motorOutput, update comments, renaming variables

thanks PL

* changes from review PL

* only permit fast re-arm if crashflip rate set and crashflip was successful

* properly exit turtle mode

* add crashFlipSuccessful to unit test extern c

* small updates from review

* improved crashflip switch handling

* remove unnecessary motors normal check

src/main/cli/settings.c

@@ -958,7 +958,7 @@ const clivalue_t valueTable[] = {
     { "yaw_motors_reversed",        VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) },
     { PARAM_NAME_MIXER_TYPE,        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) },
     { "crashflip_motor_percent",    VAR_UINT8 |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) },
-    { "crashflip_expo",             VAR_UINT8 |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) },
+    { "crashflip_rate",             VAR_UINT8 |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 250 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_rate) },
 #ifdef USE_RPM_LIMIT
     { "rpm_limit",                  VAR_INT8   |  MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit) },
     { "rpm_limit_p",                VAR_UINT16 |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 100 },        PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_p) },

src/main/cms/cms_menu_misc.c

@@ -130,6 +130,7 @@ CMS_Menu cmsx_menuRcPreview = {
 static uint16_t motorConfig_minthrottle;
 static uint8_t motorConfig_digitalIdleOffsetValue;
 static uint8_t rxConfig_fpvCamAngleDegrees;
+static uint8_t mixerConfig_crashflip_rate;
 
 static const void *cmsx_menuMiscOnEnter(displayPort_t *pDisp)
 {
@@ -138,6 +139,7 @@ static const void *cmsx_menuMiscOnEnter(displayPort_t *pDisp)
     motorConfig_minthrottle = motorConfig()->minthrottle;
     motorConfig_digitalIdleOffsetValue = motorConfig()->digitalIdleOffsetValue / 10;
     rxConfig_fpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
+    mixerConfig_crashflip_rate = mixerConfig()->crashflip_rate;
 
     return NULL;
 }
@@ -150,6 +152,7 @@ static const void *cmsx_menuMiscOnExit(displayPort_t *pDisp, const OSD_Entry *se
     motorConfigMutable()->minthrottle = motorConfig_minthrottle;
     motorConfigMutable()->digitalIdleOffsetValue = 10 * motorConfig_digitalIdleOffsetValue;
     rxConfigMutable()->fpvCamAngleDegrees = rxConfig_fpvCamAngleDegrees;
+    mixerConfigMutable()->crashflip_rate = mixerConfig_crashflip_rate;
 
     return NULL;
 }
@@ -161,6 +164,7 @@ static const OSD_Entry menuMiscEntries[]=
     { "MIN THR",        OME_UINT16 | REBOOT_REQUIRED,  NULL,          &(OSD_UINT16_t){ &motorConfig_minthrottle,            1000, 2000, 1 } },
     { "DIGITAL IDLE",   OME_UINT8 | REBOOT_REQUIRED,   NULL,          &(OSD_UINT8_t) { &motorConfig_digitalIdleOffsetValue,    0,  200, 1 } },
     { "FPV CAM ANGLE",  OME_UINT8,                     NULL,          &(OSD_UINT8_t) { &rxConfig_fpvCamAngleDegrees,           0,   90, 1 } },
+    { "CRASHFLIP RATE", OME_UINT8 | REBOOT_REQUIRED,   NULL,          &(OSD_UINT8_t) { &mixerConfig_crashflip_rate,           0,  100, 1 } },
     { "RC PREV",        OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview},
 #ifdef USE_GPS_LAP_TIMER
     { "GPS LAP TIMER",  OME_Submenu, cmsMenuChange, &cms_menuGpsLapTimer },

src/main/fc/core.c

@@ -148,7 +148,7 @@ int16_t magHold;
 
 static FAST_DATA_ZERO_INIT uint8_t pidUpdateCounter;
 
-static bool flipOverAfterCrashActive = false;
+static bool crashFlipModeActive = false;
 
 static timeUs_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
@@ -211,6 +211,15 @@ bool canUseLaunchControl(void)
 }
 #endif
 
+#ifdef USE_DSHOT
+void setMotorSpinDirection(uint8_t spinDirection)
+{
+    if (isMotorProtocolDshot() && !featureIsEnabled(FEATURE_3D)) {
+        dshotCommandWrite(ALL_MOTORS, getMotorCount(), spinDirection, DSHOT_CMD_TYPE_INLINE);
+    }
+}
+#endif
+
 void resetArmingDisabled(void)
 {
     lastArmingDisabledReason = 0;
@@ -286,9 +295,6 @@ void updateArmingStatus(void)
             unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
         }
 
-        // Clear the crash flip active status
-        flipOverAfterCrashActive = false;
-
         // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
         if (!isUsingSticksForArming()) {
             static bool hadRx = false;
@@ -319,12 +325,26 @@ void updateArmingStatus(void)
             unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
         }
 
-        if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
+        if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXCRASHFLIP)) {
             setArmingDisabled(ARMING_DISABLED_ANGLE);
         } else {
             unsetArmingDisabled(ARMING_DISABLED_ANGLE);
         }
 
+        // if, while the arm switch is enabled:
+        // - the user switches off crashflip,
+        // - and it was active, 
+        // - and the quad did not flip successfully, or we don't have that information
+        // require an arm-disarm cycle by blocking tryArm()
+        if (crashFlipModeActive && !IS_RC_MODE_ACTIVE(BOXCRASHFLIP) && !crashFlipSuccessful()) {
+            crashFlipModeActive = false;
+            // stay disarmed (motor direction normal), and block arming (require a disarm/rearm cycle)
+            setArmingDisabled(ARMING_DISABLED_CRASHFLIP);
+        } else {
+            // allow arming
+            unsetArmingDisabled(ARMING_DISABLED_CRASHFLIP);
+        }
+
 #if defined(USE_LATE_TASK_STATISTICS)
         if ((getCpuPercentageLate() > schedulerConfig()->cpuLatePercentageLimit)) {
             setArmingDisabled(ARMING_DISABLED_LOAD);
@@ -350,7 +370,7 @@ void updateArmingStatus(void)
 #ifdef USE_GPS_RESCUE
         if (gpsRescueIsConfigured()) {
             if (gpsRescueConfig()->allowArmingWithoutFix || (STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) ||
-            ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
+            ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXCRASHFLIP)) {
                 unsetArmingDisabled(ARMING_DISABLED_GPS);
             } else {
                 setArmingDisabled(ARMING_DISABLED_GPS);
@@ -438,14 +458,14 @@ void updateArmingStatus(void)
 void disarm(flightLogDisarmReason_e reason)
 {
     if (ARMING_FLAG(ARMED)) {
-        if (!flipOverAfterCrashActive) {
+        if (!crashFlipModeActive) {
             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
         }
         DISABLE_ARMING_FLAG(ARMED);
         lastDisarmTimeUs = micros();
 
 #ifdef USE_OSD
-        if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || isLaunchControlActive()) {
+        if (IS_RC_MODE_ACTIVE(BOXCRASHFLIP) || isLaunchControlActive()) {
             osdSuppressStats(true);
         }
 #endif
@@ -461,19 +481,19 @@ void disarm(flightLogDisarmReason_e reason)
 #else
         UNUSED(reason);
 #endif
+
         BEEP_OFF;
-#ifdef USE_DSHOT
+
-        if (isMotorProtocolDshot() && flipOverAfterCrashActive && !featureIsEnabled(FEATURE_3D)) {
-            dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, DSHOT_CMD_TYPE_INLINE);
-        }
-#endif
 #ifdef USE_PERSISTENT_STATS
-        if (!flipOverAfterCrashActive) {
+        if (!crashFlipModeActive) {
             statsOnDisarm();
         }
 #endif
 
-        flipOverAfterCrashActive = false;
+        // always set motor direction to normal on disarming
+#ifdef USE_DSHOT
+        setMotorSpinDirection(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+#endif
 
         // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead
         if (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))) {
@@ -488,6 +508,7 @@ void tryArm(void)
         gyroStartCalibration(true);
     }
 
+    // runs each loop while arming switches are engaged
     updateArmingStatus();
 
     if (!isArmingDisabled()) {
@@ -500,7 +521,7 @@ void tryArm(void)
 #ifdef USE_DSHOT
         if (cmpTimeUs(currentTimeUs, getLastDshotBeaconCommandTimeUs()) < DSHOT_BEACON_GUARD_DELAY_US) {
             if (tryingToArm == ARMING_DELAYED_DISARMED) {
-                if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
+                if (IS_RC_MODE_ACTIVE(BOXCRASHFLIP)) {
                     tryingToArm = ARMING_DELAYED_CRASHFLIP;
 #ifdef USE_LAUNCH_CONTROL
                 } else if (canUseLaunchControl()) {
@@ -524,28 +545,31 @@ void tryArm(void)
             }
 #endif
 
-            if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
+            // choose crashflip outcome on arming
-                // Set motor spin direction
+            // disarm can arise in processRx() if the crashflip switch is reversed while in crashflip mode
-                if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
+            // if we were unsuccessful, or cannot determin success, arming will be blocked and we can't get here
-                    flipOverAfterCrashActive = false;
+            // hence we only get here with crashFlipModeActive if the switch was reversed and result successful
-                    if (!featureIsEnabled(FEATURE_3D)) {
+            if (crashFlipModeActive) {
-                        dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, DSHOT_CMD_TYPE_INLINE);
+                // flip was successful, continue into normal flight without need to disarm/rearm
-                    }
+                // note: preceding disarm will have set motors to normal rotation direction
+                crashFlipModeActive = false; 
             } else {
-                    flipOverAfterCrashActive = true;
+                // when arming and not in crashflip mode, block entry to crashflip if delayed by the dshot beeper,
-#ifdef USE_RUNAWAY_TAKEOFF
+                // otherwise consider only the switch position
-                    runawayTakeoffCheckDisabled = false;
+                crashFlipModeActive = (tryingToArm == ARMING_DELAYED_CRASHFLIP) ? false : IS_RC_MODE_ACTIVE(BOXCRASHFLIP);
-#endif
+#ifdef USE_DSHOT
-                    if (!featureIsEnabled(FEATURE_3D)) {
+                // previous disarm will have set direction to normal
-                        dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, DSHOT_CMD_TYPE_INLINE);
+                // at this point we only need to reverse the motors if crashflipMode is active
-                    }
+                if (crashFlipModeActive) {
-                }
+                    setMotorSpinDirection(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
-            }
                 }
 #endif
+            }
+        }
+#endif // USE_DSHOT
 
 #ifdef USE_LAUNCH_CONTROL
-        if (!flipOverAfterCrashActive && (canUseLaunchControl() || (tryingToArm == ARMING_DELAYED_LAUNCH_CONTROL))) {
+        if (!crashFlipModeActive && (canUseLaunchControl() || (tryingToArm == ARMING_DELAYED_LAUNCH_CONTROL))) {
             if (launchControlState == LAUNCH_CONTROL_DISABLED) {  // only activate if it hasn't already been triggered
                 launchControlState = LAUNCH_CONTROL_ACTIVE;
             }
@@ -807,7 +831,7 @@ bool processRx(timeUs_t currentTimeUs)
     if (ARMING_FLAG(ARMED)
         && pidConfig()->runaway_takeoff_prevention
         && !runawayTakeoffCheckDisabled
-        && !flipOverAfterCrashActive
+        && !crashFlipModeActive
         && !runawayTakeoffTemporarilyDisabled
         && !isFixedWing()) {
 
@@ -959,9 +983,13 @@ void processRxModes(timeUs_t currentTimeUs)
     updateActivatedModes();
 
 #ifdef USE_DSHOT
-    /* Enable beep warning when the crash flip mode is active */
+    if (crashFlipModeActive) {
-    if (flipOverAfterCrashActive) {
+        // Enable beep warning when the crashflip mode is active
-        beeper(BEEPER_CRASH_FLIP_MODE);
+        beeper(BEEPER_CRASHFLIP_MODE);
+        if (!IS_RC_MODE_ACTIVE(BOXCRASHFLIP)) {
+            // permit the option of staying disarmed if the crashflip switch is set to off while armed
+            disarm(DISARM_REASON_SWITCH); 
+        }
     }
 #endif
 
@@ -1131,7 +1159,7 @@ static FAST_CODE_NOINLINE void subTaskPidController(timeUs_t currentTimeUs)
         && !isFixedWing()
         && pidConfig()->runaway_takeoff_prevention
         && !runawayTakeoffCheckDisabled
-        && !flipOverAfterCrashActive
+        && !crashFlipModeActive
         && !runawayTakeoffTemporarilyDisabled
         && !FLIGHT_MODE(GPS_RESCUE_MODE)   // disable Runaway Takeoff triggering if GPS Rescue is active
         && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) {
@@ -1326,9 +1354,9 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_CYCLETIME, 1, getAverageSystemLoadPercent());
 }
 
-bool isFlipOverAfterCrashActive(void)
+bool isCrashFlipModeActive(void)
 {
-    return flipOverAfterCrashActive;
+    return crashFlipModeActive;
 }
 
 timeUs_t getLastDisarmTimeUs(void)

src/main/fc/core.h

@@ -86,7 +86,7 @@ bool pidLoopReady(void);
 void taskFiltering(timeUs_t currentTimeUs);
 void taskMainPidLoop(timeUs_t currentTimeUs);
 
-bool isFlipOverAfterCrashActive(void);
+bool isCrashFlipModeActive(void);
 int8_t calculateThrottlePercent(void);
 uint8_t calculateThrottlePercentAbs(void);
 bool areSticksActive(uint8_t stickPercentLimit);

src/main/fc/rc_modes.h

@@ -62,7 +62,7 @@ typedef enum {
     BOXCAMERA1,
     BOXCAMERA2,
     BOXCAMERA3,
-    BOXFLIPOVERAFTERCRASH,
+    BOXCRASHFLIP,
     BOXPREARM,
     BOXBEEPGPSCOUNT,
     BOXVTXPITMODE,

src/main/fc/runtime_config.h

@@ -65,7 +65,8 @@ typedef enum {
     ARMING_DISABLED_DSHOT_BITBANG   = (1 << 22),
     ARMING_DISABLED_ACC_CALIBRATION = (1 << 23),
     ARMING_DISABLED_MOTOR_PROTOCOL  = (1 << 24),
-    ARMING_DISABLED_ARM_SWITCH      = (1 << 25), // Needs to be the last element, since it's always activated if one of the others is active when arming
+    ARMING_DISABLED_CRASHFLIP       = (1 << 25),
+    ARMING_DISABLED_ARM_SWITCH      = (1 << 26), // 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 (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1)

src/main/flight/gps_rescue.c

@@ -69,7 +69,7 @@ typedef enum {
     RESCUE_FLYAWAY,
     RESCUE_GPSLOST,
     RESCUE_LOWSATS,
-    RESCUE_CRASH_FLIP_DETECTED,
+    RESCUE_CRASHFLIP_DETECTED,
     RESCUE_STALLED,
     RESCUE_TOO_CLOSE,
     RESCUE_NO_HOME_POINT

src/main/flight/imu.c

@@ -98,7 +98,7 @@ static vector2_t north_ef;
 
 #if defined(USE_ACC)
 STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
-#endif
+#endif // USE_ACC
 
 // quaternion of sensor frame relative to earth frame
 STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;

src/main/flight/mixer.c

@@ -62,12 +62,16 @@
 
 #include "sensors/battery.h"
 #include "sensors/gyro.h"
+#include "sensors/sensors.h"
 
 #include "mixer.h"
 
 #define DYN_LPF_THROTTLE_STEPS             100
 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US  5000 // minimum of 5ms between updates
 
+#define CRASHFLIP_MOTOR_DEADBAND         0.02f // 2%; send 'disarm' value to motors below this drive value
+#define CRASHFLIP_STICK_DEADBAND         0.15f // 15%
+
 static FAST_DATA_ZERO_INIT float motorMixRange;
 
 float FAST_DATA_ZERO_INIT motor[MAX_SUPPORTED_MOTORS];
@@ -108,6 +112,7 @@ static FAST_DATA_ZERO_INIT float motorRangeMin;
 static FAST_DATA_ZERO_INIT float motorRangeMax;
 static FAST_DATA_ZERO_INIT float motorOutputRange;
 static FAST_DATA_ZERO_INIT int8_t motorOutputMixSign;
+static FAST_DATA_ZERO_INIT bool crashflipSuccess = false;
 
 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 {
@@ -139,7 +144,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
         const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
         const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
 
-        if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) {
+        if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isCrashFlipModeActive()) {
             // INVERTED
             motorRangeMin = mixerRuntime.motorOutputLow;
             motorRangeMax = mixerRuntime.deadbandMotor3dLow;
@@ -255,7 +260,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
             DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100));
             DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000));
         }
-        motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
+        motorRangeMax = isCrashFlipModeActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
 #else
         motorRangeMax = mixerRuntime.motorOutputHigh;
 #endif
@@ -269,32 +274,36 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
     rcThrottle = throttle;
 }
 
-#define CRASH_FLIP_DEADBAND 20
+static bool applyCrashFlipModeToMotors(void)
-#define CRASH_FLIP_STICK_MINF 0.15f
-
-static void applyFlipOverAfterCrashModeToMotors(void)
 {
-    if (ARMING_FLAG(ARMED)) {
+#ifdef USE_ACC
-        const float flipPowerFactor = 1.0f - mixerConfig()->crashflip_expo / 100.0f;
+    static bool isTiltAngleAtStartSet = false;
+    static float tiltAngleAtStart = 1.0f;
+#endif
+
+    if (!isCrashFlipModeActive()) {
+#ifdef USE_ACC
+        // trigger the capture of initial tilt angle on next activation of crashflip mode
+        isTiltAngleAtStartSet = false;
+        // default the success flag to false, to block quick re-arming unless successful
+#endif
+        // signal that crashflip mode is off
+        return false;
+    }
+
     const float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH);
     const float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL);
     const float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW);
 
-        const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor);
-        const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor);
-        const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor);
-
     float signPitch = getRcDeflection(FD_PITCH) < 0 ? 1 : -1;
     float signRoll = getRcDeflection(FD_ROLL) < 0 ? 1 : -1;
     float signYaw = (getRcDeflection(FD_YAW) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1);
 
     float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
-        float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
 
     if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
         // If yaw is the dominant, disable pitch and roll
         stickDeflectionLength = stickDeflectionYawAbs;
-            stickDeflectionExpoLength = stickDeflectionYawExpo;
         signRoll = 0;
         signPitch = 0;
     } else {
@@ -303,7 +312,7 @@ static void applyFlipOverAfterCrashModeToMotors(void)
     }
 
     const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0;
-        const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
+    const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(30 deg)
 
     if (cosPhi < cosThreshold) {
         // Enforce either roll or pitch exclusively, if not on diagonal
@@ -314,10 +323,50 @@ static void applyFlipOverAfterCrashModeToMotors(void)
         }
     }
 
-        // Apply a reasonable amount of stick deadband
+    // Calculate crashflipPower from stick deflection with a reasonable amount of stick deadband
-        const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor);
+    float crashflipPower = stickDeflectionLength > CRASHFLIP_STICK_DEADBAND ? stickDeflectionLength : 0.0f;
-        const float flipStickRange = 1.0f - crashFlipStickMinExpo;
+
-        const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
+    // calculate flipPower attenuators
+    float crashflipRateAttenuator = 1.0f;
+    float crashflipAttitudeAttenuator = 1.0f;
+    const float crashflipRateLimit = mixerConfig()->crashflip_rate * 10.0f; // eg 35 = no power by 350 deg/s
+    const float halfComplete = 0.5f; // attitude or rate changes less that this will be ignored
+
+    // disable both attenuators if the user's crashflip_rate is zero
+    if (crashflipRateLimit > 0) {
+#ifdef USE_ACC
+        // Calculate an attenuator based on change of attitude (requires Acc)
+        // with Acc, crashflipAttitudeAttenuator will be zero after approx 90 degree rotation, and
+        // motors will stop / not spin while attitude remains more than ~90 degrees from initial attitude
+        // without Acc, the user must manually center the stick, or exit crashflip mode, or disarm, to stop the motors
+        // re-initialisation of crashFlip mode by arm/disarm is required to reset the initial tilt angle
+        if (sensors(SENSOR_ACC)) {
+            const float tiltAngle = getCosTiltAngle();  // -1 if flat inverted, 0 when 90° (on edge), +1 when flat and upright
+            if (!isTiltAngleAtStartSet) {
+                tiltAngleAtStart = tiltAngle;
+                isTiltAngleAtStartSet = true;
+                crashflipSuccess = false;
+            }
+            // attitudeChangeNeeded is 1.0 at the start, decreasing to 0 when attitude change exceeds approx 90 degrees
+            const float attitudeChangeNeeded = fmaxf(1.0f - fabsf(tiltAngle - tiltAngleAtStart), 0.0f);
+            // no attenuation unless a significant attitude change has occurred
+            crashflipAttitudeAttenuator = attitudeChangeNeeded > halfComplete ? 1.0f : attitudeChangeNeeded / halfComplete;
+
+            // signal success to enable quick restart, if attitude change implies success when reverting the switch
+            crashflipSuccess = attitudeChangeNeeded == 0.0f;
+        }
+#endif // USE_ACC
+        // Calculate an attenuation factor based on rate of rotation... note:
+        // if driving roll or pitch, quad usually turns on that axis, but if one motor sticks, could be a diagonal rotation
+        // if driving diagonally, the turn could be either roll or pitch
+        // if driving yaw, typically one motor sticks, and the quad yaws a little then flips diagonally
+        const float gyroRate = fmaxf(fabsf(gyro.gyroADCf[FD_ROLL]), fabsf(gyro.gyroADCf[FD_PITCH]));
+        const float gyroRateChange = fminf(gyroRate / crashflipRateLimit, 1.0f);
+        // no attenuation unless a significant gyro rate change has occurred
+        crashflipRateAttenuator = gyroRateChange < halfComplete ? 1.0f : (1.0f - gyroRateChange) / halfComplete;
+
+        crashflipPower *= crashflipAttitudeAttenuator * crashflipRateAttenuator;
+    }
 
     for (int i = 0; i < mixerRuntime.motorCount; ++i) {
         float motorOutputNormalised =
@@ -332,20 +381,18 @@ static void applyFlipOverAfterCrashModeToMotors(void)
                 motorOutputNormalised = 0;
             }
         }
-            motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
+
+        motorOutputNormalised = MIN(1.0f, crashflipPower * motorOutputNormalised);
         float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange;
 
-            // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
+        // set motors to disarm value when intended increase is less than deadband value
-            motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? mixerRuntime.disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND);
+        motorOutput = (motorOutputNormalised < CRASHFLIP_MOTOR_DEADBAND) ? mixerRuntime.disarmMotorOutput : motorOutput;
         
         motor[i] = motorOutput;
     }
-    } else {
+
-        // Disarmed mode
+    // signal that crashflip mode has been applied to motors
-        for (int i = 0; i < mixerRuntime.motorCount; i++) {
+    return true;
-            motor[i] = motor_disarmed[i];
-        }
-    }
 }
 
 #ifdef USE_RPM_LIMIT
@@ -607,9 +654,8 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
     // Find min and max throttle based on conditions. Throttle has to be known before mixing
     calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
 
-    if (isFlipOverAfterCrashActive()) {
+    if (applyCrashFlipModeToMotors()) {
-        applyFlipOverAfterCrashModeToMotors();
+        return; // if crash flip mode has been applied to the motors, mixing is done
-        return;
     }
 
     const bool launchControlActive = isLaunchControlActive();
@@ -786,3 +832,8 @@ float mixerGetRcThrottle(void)
 {
     return rcThrottle;
 }
+
+bool crashFlipSuccessful(void)
+{
+    return crashflipSuccess;
+}

src/main/flight/mixer.h

@@ -95,7 +95,7 @@ typedef struct mixerConfig_s {
     uint8_t mixerMode;
     bool yaw_motors_reversed;
     uint8_t crashflip_motor_percent;
-    uint8_t crashflip_expo;
+    uint8_t crashflip_rate;
     uint8_t mixer_type;
 #ifdef USE_RPM_LIMIT
     bool rpm_limit;
@@ -147,3 +147,4 @@ bool isFixedWing(void);
 
 float getMotorOutputLow(void);
 float getMotorOutputHigh(void);
+bool crashFlipSuccessful(void);

src/main/flight/mixer_init.c

@@ -54,7 +54,11 @@ void pgResetFn_mixerConfig(mixerConfig_t *mixerConfig)
     mixerConfig->mixerMode = DEFAULT_MIXER;
     mixerConfig->yaw_motors_reversed = false;
     mixerConfig->crashflip_motor_percent = 0;
-    mixerConfig->crashflip_expo = 35;
+#ifdef USE_RACE_PRO
+    mixerConfig->crashflip_rate = 30;
+#else
+    mixerConfig->crashflip_rate = 0;
+#endif
     mixerConfig->mixer_type = MIXER_LEGACY;
 #ifdef USE_RPM_LIMIT
     mixerConfig->rpm_limit = false;

src/main/io/beeper.c

@@ -230,7 +230,7 @@ static const beeperTableEntry_t beeperTable[] = {
     { BEEPER_ENTRY(BEEPER_SYSTEM_INIT,           17, NULL,                 "SYSTEM_INIT") },
     { BEEPER_ENTRY(BEEPER_USB,                   18, NULL,                 "ON_USB") },
     { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE,        19, beep_2shortBeeps,     "BLACKBOX_ERASE") },
-    { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE,       20, beep_2longerBeeps,    "CRASH_FLIP") },
+    { BEEPER_ENTRY(BEEPER_CRASHFLIP_MODE,        20, beep_2longerBeeps,    "CRASHFLIP") },
     { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN,   21, beep_camOpenBeep,     "CAM_CONNECTION_OPEN") },
     { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE,  22, beep_camCloseBeep,    "CAM_CONNECTION_CLOSE") },
     { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,23, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") },

src/main/io/beeper.h

@@ -56,7 +56,7 @@ typedef enum {
     BEEPER_SYSTEM_INIT,             // Initialisation beeps when board is powered on
     BEEPER_USB,                     // Some boards have beeper powered USB connected
     BEEPER_BLACKBOX_ERASE,          // Beep when blackbox erase completes
-    BEEPER_CRASH_FLIP_MODE,         // Crash flip mode is active
+    BEEPER_CRASHFLIP_MODE,          // Crashflip mode is active
     BEEPER_CAM_CONNECTION_OPEN,     // When the 5 key simulation stated
     BEEPER_CAM_CONNECTION_CLOSE,    // When the 5 key simulation stop
     BEEPER_RC_SMOOTHING_INIT_FAIL,  // Warning beep pattern when armed and rc smoothing has not initialized filters
@@ -87,7 +87,7 @@ STATIC_ASSERT(BEEPER_ALL < sizeof(uint32_t) * 8, "BEEPER bits exhausted");
     | BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT) \
     | BEEPER_GET_FLAG(BEEPER_USB) \
     | BEEPER_GET_FLAG(BEEPER_BLACKBOX_ERASE) \
-    | BEEPER_GET_FLAG(BEEPER_CRASH_FLIP_MODE) \
+    | BEEPER_GET_FLAG(BEEPER_CRASHFLIP_MODE) \
     | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_OPEN) \
     | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_CLOSE) \
     | BEEPER_GET_FLAG(BEEPER_RC_SMOOTHING_INIT_FAIL) \

src/main/io/ledstrip.c

@@ -690,7 +690,7 @@ typedef enum {
     WARNING_ARMING_DISABLED,
     WARNING_LOW_BATTERY,
     WARNING_FAILSAFE,
-    WARNING_CRASH_FLIP_ACTIVE,
+    WARNING_CRASHFLIP_ACTIVE,
 } warningFlags_e;
 
 static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
@@ -714,8 +714,8 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
             if (!ARMING_FLAG(ARMED) && isArmingDisabled()) {
                 warningFlags |= 1 << WARNING_ARMING_DISABLED;
             }
-            if (isFlipOverAfterCrashActive()) {
+            if (isCrashFlipModeActive()) {
-                warningFlags |= 1 << WARNING_CRASH_FLIP_ACTIVE;
+                warningFlags |= 1 << WARNING_CRASHFLIP_ACTIVE;
             }
         }
         *timer += HZ_TO_US(LED_OVERLAY_WARNING_RATE_HZ);
@@ -731,7 +731,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
                 case WARNING_ARMING_DISABLED:
                     warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK);
                     break;
-                case WARNING_CRASH_FLIP_ACTIVE:
+                case WARNING_CRASHFLIP_ACTIVE:
                     warningColor = colorOn ? &HSV(MAGENTA) : &HSV(BLACK);
                     break;
                 case WARNING_LOW_BATTERY:

src/main/msp/msp_box.c

@@ -82,7 +82,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { .boxId = BOXCAMERA1, .boxName = "CAMERA CONTROL 1", .permanentId = 32},
     { .boxId = BOXCAMERA2, .boxName = "CAMERA CONTROL 2", .permanentId = 33},
     { .boxId = BOXCAMERA3, .boxName = "CAMERA CONTROL 3", .permanentId = 34 },
-    { .boxId = BOXFLIPOVERAFTERCRASH, .boxName = "FLIP OVER AFTER CRASH", .permanentId = 35 },
+    { .boxId = BOXCRASHFLIP, .boxName = "FLIP OVER AFTER CRASH", .permanentId = 35 },
     { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 36 },
     { .boxId = BOXBEEPGPSCOUNT, .boxName = "GPS BEEP SATELLITE COUNT", .permanentId = 37 },
 //    { .boxId = BOX3DONASWITCH, .boxName = "3D ON A SWITCH", .permanentId = 38 }, (removed)
@@ -283,7 +283,7 @@ void initActiveBoxIds(void)
     bool configuredMotorProtocolDshot;
     checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot);
     if (configuredMotorProtocolDshot) {
-        BME(BOXFLIPOVERAFTERCRASH);
+        BME(BOXCRASHFLIP);
     }
 #endif
 

src/main/osd/osd.c

@@ -1201,8 +1201,8 @@ static timeDelta_t osdShowArmed(void)
     }
     displayWrite(osdDisplayPort, midCol - (strlen("ARMED") / 2), midRow, DISPLAYPORT_SEVERITY_NORMAL, "ARMED");
 
-    if (isFlipOverAfterCrashActive()) {
+    if (isCrashFlipModeActive()) {
-        displayWrite(osdDisplayPort, midCol - (strlen(CRASH_FLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASH_FLIP_WARNING);
+        displayWrite(osdDisplayPort, midCol - (strlen(CRASHFLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASHFLIP_WARNING);
     }
 
     return ret;
@@ -1287,9 +1287,9 @@ void osdProcessStats2(timeUs_t currentTimeUs)
 
     if (resumeRefreshAt) {
         if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
-            // in timeout period, check sticks for activity or CRASH FLIP switch to resume display.
+            // in timeout period, check sticks for activity or CRASHFLIP switch to resume display.
             if (!ARMING_FLAG(ARMED) &&
-                (IS_HI(THROTTLE) || IS_HI(PITCH) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH))) {
+                (IS_HI(THROTTLE) || IS_HI(PITCH) || IS_RC_MODE_ACTIVE(BOXCRASHFLIP))) {
                 resumeRefreshAt = currentTimeUs;
             }
             return;

src/main/osd/osd.h

@@ -269,7 +269,7 @@ typedef enum {
     OSD_WARNING_BATTERY_WARNING,
     OSD_WARNING_BATTERY_CRITICAL,
     OSD_WARNING_VISUAL_BEEPER,
-    OSD_WARNING_CRASH_FLIP,
+    OSD_WARNING_CRASHFLIP,
     OSD_WARNING_ESC_FAIL,
     OSD_WARNING_CORE_TEMPERATURE,
     OSD_WARNING_RC_SMOOTHING,

src/main/osd/osd_elements.c

@@ -887,7 +887,7 @@ static void osdElementCrashFlipArrow(osdElementParms_t *element)
         rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
     }
 
-    if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) {
+    if ((isCrashFlipModeActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) {
         if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
             if (pitchAngle > 0) {
                 if (rollAngle > 0) {

src/main/osd/osd_warnings.c

@@ -64,7 +64,7 @@
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
 
-const char CRASH_FLIP_WARNING[] = "> CRASH FLIP <";
+const char CRASHFLIP_WARNING[] = "> CRASH FLIP <";
 
 void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 {
@@ -135,12 +135,12 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     }
 
     // Warn when in flip over after crash mode
-    if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
+    if (osdWarnGetState(OSD_WARNING_CRASHFLIP) && IS_RC_MODE_ACTIVE(BOXCRASHFLIP)) {
-        if (isFlipOverAfterCrashActive()) { // if was armed in crash flip mode
+        if (isCrashFlipModeActive()) { // if was armed in crashflip mode
-            tfp_sprintf(warningText, CRASH_FLIP_WARNING);
+            tfp_sprintf(warningText, CRASHFLIP_WARNING);
             *displayAttr = DISPLAYPORT_SEVERITY_INFO;
             return;
-        } else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crash flip mode is activated
+        } else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crashflip mode is activated (not allowed / can't happen)
             tfp_sprintf(warningText, "CRASH FLIP SWITCH");
             *displayAttr = DISPLAYPORT_SEVERITY_INFO;
             return;

src/main/osd/osd_warnings.h

@@ -23,7 +23,7 @@
 #define OSD_WARNINGS_MAX_SIZE 12
 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
 
-extern const char CRASH_FLIP_WARNING[];
+extern const char CRASHFLIP_WARNING[];
 
 STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= OSD_ELEMENT_BUFFER_LENGTH, osd_warnings_size_exceeds_buffer_size);
 

src/main/rx/rc_stats.c

@@ -49,13 +49,13 @@ void rcStatsUpdate(timeUs_t currentTimeUs)
     previousTimeUs = currentTimeUs;
     const int8_t throttlePercent = calculateThrottlePercent();
 
-    if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && !throttleEverRaisedAfterArming) {
+    if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXCRASHFLIP) && !throttleEverRaisedAfterArming) {
         if (abs(throttlePercent) >= 15) { // start counting stats if throttle was raised >= 15% after arming
             throttleEverRaisedAfterArming = true;
         }
     }
 
-    if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && throttleEverRaisedAfterArming) {
+    if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXCRASHFLIP) && throttleEverRaisedAfterArming) {
         counter++;
         totalTrottleNumber += throttlePercent;
 

src/test/unit/arming_prevention_unittest.cc

@@ -1161,5 +1161,6 @@ extern "C" {
     }
     void getRcDeflectionAbs(void) {}
     uint32_t getCpuPercentageLate(void) { return 0; }
-    bool isAltitudeLow(void) {return false ;};
+    bool crashFlipSuccessful(void) {return false; }
+    bool isAltitudeLow(void) {return false; }
 }

src/test/unit/ledstrip_unittest.cc

@@ -403,7 +403,7 @@ bool isArmingDisabled(void) { return false; }
 
 uint8_t getRssiPercent(void) { return 0; }
 
-bool isFlipOverAfterCrashActive(void) { return false; }
+bool isCrashFlipModeActive(void) { return false; }
 
 void ws2811LedStripEnable(void) { }
 

src/test/unit/link_quality_unittest.cc

@@ -494,7 +494,7 @@ extern "C" {
     bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
     bool cmsDisplayPortRegister(displayPort_t *) { return false; }
     uint16_t getCoreTemperatureCelsius(void) { return 0; }
-    bool isFlipOverAfterCrashActive(void) { return false; }
+    bool isCrashFlipModeActive(void) { return false; }
     float pidItermAccelerator(void) { return 1.0; }
     uint8_t getMotorCount(void){ return 4; }
     bool areMotorsRunning(void){ return true; }

src/test/unit/osd_unittest.cc

@@ -1396,7 +1396,7 @@ extern "C" {
 
     uint16_t getCoreTemperatureCelsius(void) { return simulationCoreTemperature; }
 
-    bool isFlipOverAfterCrashActive(void) { return false; }
+    bool isCrashFlipModeActive(void) { return false; }
 
     float pidItermAccelerator(void) { return 1.0; }
     uint8_t getMotorCount(void){ return 4; }

src/test/unit/vtx_unittest.cc

@@ -211,4 +211,6 @@ extern "C" {
     void sbufWriteU16(sbuf_t *, uint16_t) {}
     void sbufWriteU32(sbuf_t *, uint32_t) {}
     void schedulerSetNextStateTime(timeDelta_t) {}
+    bool crashFlipSuccessful(void) {return false; }
+
 }