Merge pull request #4004 from brycedjohnson/Turtle2prop

Use stick deflection for turtle mode

src/main/fc/fc_core.c

@@ -109,6 +109,8 @@ int16_t magHold;
 int16_t headFreeModeHold;
 
 static bool reverseMotors = false;
+static bool flipOverAfterCrashMode = false;
+
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
 bool isRXDataNew;
@@ -261,14 +263,14 @@ void tryArm(void)
             return;
         }
 #ifdef USE_DSHOT
-        if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXDSHOTREVERSE)) {
+        if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
             pwmDisableMotors();
 
-            if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) {
-                reverseMotors = false;
+            if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
+                flipOverAfterCrashMode = false;
                 pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL);
             } else {
-                reverseMotors = true;
+                flipOverAfterCrashMode = true;
                 pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED);
             }
 
@@ -723,7 +725,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     }
 }
 
-bool isMotorsReversed()
+bool isMotorsReversed(void)
 {
     return reverseMotors;
 }
+bool isFlipOverAfterCrashMode(void)
+{
+    return flipOverAfterCrashMode;
+}

src/main/fc/fc_core.h

@@ -49,3 +49,4 @@ void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
 bool isMotorsReversed(void);
+bool isFlipOverAfterCrashMode(void);

src/main/fc/fc_msp_box.c

@@ -76,7 +76,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXCAMERA1, "CAMERA CONTROL 1", 32},
     { BOXCAMERA2, "CAMERA CONTROL 2", 33},
     { BOXCAMERA3, "CAMERA CONTROL 3", 34 },
-    { BOXDSHOTREVERSE, "DSHOT REVERSE MOTORS", 35 },
+    { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH (DSHOT ONLY)", 35 },
     { BOXPREARM, "PREARM", 36 },
 };
 
@@ -217,7 +217,7 @@ void initActiveBoxIds(void)
     }
 
     if (isMotorProtocolDshot()) {
-        BME(BOXDSHOTREVERSE);
+        BME(BOXFLIPOVERAFTERCRASH);
     }
 
     if (feature(FEATURE_SERVO_TILT)) {
@@ -290,7 +290,7 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
     const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
         | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD)
         | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE)
-        | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXDSHOTREVERSE) | BM(BOX3DDISABLE);
+        | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXFLIPOVERAFTERCRASH) | BM(BOX3DDISABLE);
     STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes);
     for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
         if ((rcModeCopyMask & BM(i))    // mode copy is enabled

src/main/fc/rc_modes.h

@@ -57,7 +57,7 @@ typedef enum {
     BOXCAMERA1,
     BOXCAMERA2,
     BOXCAMERA3,
-    BOXDSHOTREVERSE,
+    BOXFLIPOVERAFTERCRASH,
     BOXPREARM,
     CHECKBOX_ITEM_COUNT
 } boxId_e;

src/main/flight/mixer.c

@@ -43,6 +43,7 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 #include "fc/fc_core.h"
+#include "fc/fc_rc.h"
 
 #include "flight/failsafe.h"
 #include "flight/imu.h"
@@ -563,6 +564,34 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
     motorOutputRange = motorOutputMax - motorOutputMin;
 }
 
+static void applyFlipOverAfterCrashModeToMotors(void)
+{
+    float motorMix[MAX_SUPPORTED_MOTORS];
+
+    for (int i = 0; i < motorCount; i++) {
+        if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) {
+            motorMix[i] = getRcDeflection(FD_ROLL) * pidSumLimit * currentMixer[i].roll * (-1);
+        } else {
+            motorMix[i] = getRcDeflection(FD_PITCH) * pidSumLimit * currentMixer[i].pitch * (-1);
+        }
+    }
+    // Apply the mix to motor endpoints
+    for (uint32_t i = 0; i < motorCount; i++) {
+        float motorOutput =  motorOutputMin + motorOutputRange * (motorMix[i]);
+        //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
+        motorOutput = (motorOutput < motorOutputMin + 20 ) ? disarmMotorOutput : motorOutput;
+
+        motor[i] = motorOutput;
+    }
+
+    // Disarmed mode
+    if (!ARMING_FLAG(ARMED)) {
+        for (int i = 0; i < motorCount; i++) {
+            motor[i] = motor_disarmed[i];
+        }
+    }
+}
+
 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
 {
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
@@ -604,6 +633,10 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
 
 void mixTable(uint8_t vbatPidCompensation)
 {
+    if (isFlipOverAfterCrashMode()) {
+        applyFlipOverAfterCrashModeToMotors();        
+        return;
+    }
     // Find min and max throttle based on conditions. Throttle has to be known before mixing
     calculateThrottleAndCurrentMotorEndpoints();