Flight mode implemented but doesn't work

src/main/common/maths.h

@@ -88,6 +88,8 @@
 #define _ABS_I(x, var) _ABS_II(x, var)
 #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
 
+#define power3(x) ((x)*(x)*(x))
+
 // Floating point Euler angles.
 typedef struct fp_angles {
     float roll;

src/main/drivers/pwm_output.h

@@ -28,6 +28,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value);
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
 void pwmCompleteMotorUpdate(void);
 bool isMotorProtocolDigital(void);
+bool isMotorProtocolDshot(void);
 
 void pwmWriteServo(uint8_t index, uint16_t value);
 

src/main/fc/fc_core.c

@@ -398,6 +398,9 @@ void disarm(disarmReason_t disarmReason)
             blackboxFinish();
         }
 #endif
+        if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+            DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
+        }
 
         statsOnDisarm();
         logicConditionReset();
@@ -457,6 +460,20 @@ void releaseSharedTelemetryPorts(void) {
 void tryArm(void)
 {
     updateArmingStatus();
+
+#ifdef USE_DSHOT
+    if (
+            STATE(MULTIROTOR) &&
+            IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
+            isMotorProtocolDshot() &&
+            !ARMING_FLAG(ARMING_DISABLED_THROTTLE) &&
+            !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
+            ) {
+        ENABLE_ARMING_FLAG(ARMED);
+        enableFlightMode(FLIP_OVER_AFTER_CRASH);
+        return;
+    }
+#endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
     if (
         !isArmingDisabled() || 

src/main/fc/fc_msp_box.c

@@ -87,6 +87,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
     { BOXPREARM, "PREARM", 51 },
+    { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -306,6 +307,10 @@ void initActiveBoxIds(void)
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE;
 #endif
+
+#ifdef USE_DSHOT
+    activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
+#endif
 }
 
 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)

src/main/fc/rc_modes.h

@@ -69,6 +69,7 @@ typedef enum {
     BOXLOITERDIRCHN  = 40,
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
+    BOXFLIPOVERAFTERCRASH = 43,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/runtime_config.h

@@ -97,6 +97,7 @@ typedef enum {
     NAV_CRUISE_MODE = (1 << 12),
     FLAPERON        = (1 << 13),
     TURN_ASSISTANT  = (1 << 14),
+    FLIP_OVER_AFTER_CRASH = (1 << 15),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;

src/main/flight/mixer.c

@@ -319,6 +319,100 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
+
+#define CRASH_FLIP_DEADBAND 20.0f
+#define CRASH_FLIP_STICK_MINF 0.15f
+
+static void applyFlipOverAfterCrashModeToMotors(void)
+{
+    //To convert in configs
+    const float crashlip_expo = 35.0f;
+
+    if (ARMING_FLAG(ARMED)) {
+        const float flipPowerFactor = 1.0f - crashlip_expo / 100.0f;
+        const float stickDeflectionPitchAbs = ABS(((float)rcCommand[PITCH])/500.0f);
+        const float stickDeflectionRollAbs = ABS(((float)rcCommand[ROLL])/500.0f);
+        const float stickDeflectionYawAbs = ABS(((float)rcCommand[YAW])/500.0f);
+        //deflection stick position
+
+        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 = rcCommand[PITCH] < 0 ? 1 : -1;
+        float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
+        //float signYaw = (rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1);
+        float signYaw = (rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 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 {
+            // If pitch/roll dominant, disable yaw
+            signYaw = 0;
+        }
+
+        const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0;
+        const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
+
+        if (cosPhi < cosThreshold) {
+            // Enforce either roll or pitch exclusively, if not on diagonal
+            if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
+                signPitch = 0;
+            } else {
+                signRoll = 0;
+            }
+        }
+
+        // Apply a reasonable amount of stick deadband
+        const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor);
+        const float flipStickRange = 1.0f - crashFlipStickMinExpo;
+        const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
+
+        for (int i = 0; i < motorCount; ++i) {
+
+            float motorOutputNormalised =
+                signPitch * currentMixer[i].pitch +    //mixer, per ogni motore quanto interviene nel pitch, roll e yaw
+                signRoll * currentMixer[i].roll +
+                signYaw * currentMixer[i].yaw;
+
+            if (motorOutputNormalised < 0) {
+                    motorOutputNormalised = 0;
+            }
+
+            motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
+
+            float motorOutput = motorConfig()->mincommand+ motorOutputNormalised * motorConfig()->maxthrottle;
+
+            // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
+            motorOutput = (motorOutput < motorConfig()->mincommand + CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (motorOutput - CRASH_FLIP_DEADBAND);
+
+            uint16_t motorValue;
+
+            motorValue = handleOutputScaling(
+                    motorOutput,
+                    throttleIdleValue,
+                    DSHOT_DISARM_COMMAND,
+                    motorConfig()->mincommand,
+                    motorConfig()->maxthrottle,
+                    DSHOT_MIN_THROTTLE,
+                    DSHOT_3D_DEADBAND_LOW,
+                    false
+            );
+
+            pwmWriteMotor(i,motorValue);
+        }
+    } else {
+        // Disarmed mode
+        stopMotors();
+    }
+}
 #endif
 
 void FAST_CODE writeMotors(void)
@@ -443,6 +537,11 @@ static int getReversibleMotorsThrottleDeadband(void)
 
 void FAST_CODE mixTable(const float dT)
 {
+    if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+        applyFlipOverAfterCrashModeToMotors();
+        return;
+    }
+
     int16_t input[3];   // RPY, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes
     if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {

src/main/io/osd.c

@@ -1609,6 +1609,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
+            else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
+                p = "TURT";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
             return true;