Flip over after crash aka TURTLE MODE

docs/Settings.md

@@ -101,6 +101,7 @@
 | failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
 | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
+| flip_over_after_crash_power_factor | 65 | flip over after crash power factor |
 | fpv_mix_degrees |  |  |
 | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
 | frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |

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.c

@@ -60,6 +60,8 @@ FILE_COMPILE_FOR_SPEED
 #define DSHOT_MOTOR_BITLENGTH   20
 
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
+
+#define DSHOT_COMMAND_DELAY_US 1000
 #endif
 
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
@@ -568,6 +570,24 @@ void pwmWriteServo(uint8_t index, uint16_t value)
     }
 }
 
+#ifdef USE_DSHOT
+void changeDshotSpinRotation(dshotDirectionCommands_e directionSpin) {
+
+    const uint8_t number_of_packets = 10;
+
+    for (uint8_t j = 0; j < number_of_packets; j++) {
+
+        delayMicroseconds(DSHOT_COMMAND_DELAY_US);
+        for (uint8_t i = 0; i < getMotorCount(); i++) {
+            pwmRequestMotorTelemetry(i);
+            pwmWriteMotor(i, directionSpin);
+        }
+        pwmCompleteMotorUpdate();
+
+    }
+}
+#endif
+
 #ifdef BEEPER_PWM
 void pwmWriteBeeper(bool onoffBeep)
 {

src/main/drivers/pwm_output.h

@@ -20,6 +20,11 @@
 #include "drivers/io_types.h"
 #include "drivers/time.h"
 
+typedef enum {
+    DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
+    DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
+} dshotDirectionCommands_e;
+
 void pwmRequestMotorTelemetry(int motorIndex);
 
 ioTag_t pwmGetMotorPinTag(int motorIndex);
@@ -28,6 +33,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);
 
@@ -41,4 +47,6 @@ bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIn
 void pwmServoPreconfigure(void);
 bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
 void pwmWriteBeeper(bool onoffBeep);
-void beeperPwmInit(ioTag_t tag, uint16_t frequency);
+void beeperPwmInit(ioTag_t tag, uint16_t frequency);
+
+void changeDshotSpinRotation(dshotDirectionCommands_e directionSpin);

src/main/fc/controlrate_profile.c

@@ -64,7 +64,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
             },
 
             .misc = {
-                .fpvCamAngleDegrees = 0
+                .fpvCamAngleDegrees = 0,
             }
         );
     }

src/main/fc/fc_core.c

@@ -398,7 +398,12 @@ void disarm(disarmReason_t disarmReason)
             blackboxFinish();
         }
 #endif
-
+#ifdef USE_DSHOT
+        if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+            changeDshotSpinRotation(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+            DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
+        }
+#endif
         statsOnDisarm();
         logicConditionReset();
         programmingPidReset();
@@ -457,6 +462,22 @@ 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)
+            ) {
+
+        changeDshotSpinRotation(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+        ENABLE_ARMING_FLAG(ARMED);
+        enableFlightMode(FLIP_OVER_AFTER_CRASH);
+        return;
+    }
+#endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
     if (
         !isArmingDisabled() || 

src/main/fc/fc_msp_box.c

@@ -33,6 +33,8 @@
 
 #include "io/osd.h"
 
+#include "drivers/pwm_output.h"
+
 #include "sensors/diagnostics.h"
 #include "sensors/sensors.h"
 
@@ -87,6 +89,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 +309,11 @@ void initActiveBoxIds(void)
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE;
 #endif
+
+#ifdef USE_DSHOT
+    if(STATE(MULTIROTOR) && isMotorProtocolDshot())
+        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;
 
@@ -128,4 +129,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 
 void updateActivatedModes(void);
 void updateUsedModeActivationConditionFlags(void);
-void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);
+void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

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/fc/settings.yaml

@@ -722,6 +722,12 @@ groups:
         default_value: "14"
         min: 4
         max: 255
+      - name: flip_over_after_crash_power_factor
+        field: flipOverAfterPowerFactor
+        default_value: "65"
+        description: "flip over after crash power factor"
+        min: 0
+        max: 100
 
   - name: PG_FAILSAFE_CONFIG
     type: failsafeConfig_t

src/main/flight/mixer.c

@@ -42,6 +42,7 @@ FILE_COMPILE_FOR_SPEED
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
+#include "fc/controlrate_profile.h"
 
 #include "flight/failsafe.h"
 #include "flight/imu.h"
@@ -99,22 +100,26 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
 
 #define DEFAULT_MAX_THROTTLE    1850
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
     .motorPwmRate = DEFAULT_PWM_RATE,
     .maxthrottle = DEFAULT_MAX_THROTTLE,
-    .mincommand = 1000, 
+    .mincommand = 1000,
     .motorAccelTimeMs = 0,
     .motorDecelTimeMs = 0,
     .throttleIdle = 15.0f,
     .throttleScale = 1.0f,
-    .motorPoleCount = 14            // Most brushless motors that we use are 14 poles
+    .motorPoleCount = 14,           // Most brushless motors that we use are 14 poles
+    .flipOverAfterPowerFactor = 65
 );
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
+#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
+#define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
+
 typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
 static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
 
@@ -184,7 +189,7 @@ void mixerUpdateStateFlags(void)
     } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) {
         ENABLE_STATE(MULTIROTOR);
         ENABLE_STATE(ALTITUDE_CONTROL);
-    } 
+    }
 
     if (mixerConfig()->hasFlaps) {
         ENABLE_STATE(FLAPERON_AVAILABLE);
@@ -266,7 +271,7 @@ void mixerInit(void)
 void mixerResetDisarmedMotors(void)
 {
     int motorZeroCommand;
-    
+
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
         motorZeroCommand = reversibleMotorsConfig()->neutral;
         throttleRangeMin = throttleDeadbandHigh;
@@ -319,6 +324,85 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
+static void applyFlipOverAfterCrashModeToMotors(void) {
+
+    if (ARMING_FLAG(ARMED)) {
+        const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/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 = (float)((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_OVER_AFTER_CRASH_FLIP_STICK_MIN + power3(CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN) * (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 +
+                    signRoll * currentMixer[i].roll +
+                    signYaw * currentMixer[i].yaw;
+
+            if (motorOutputNormalised < 0) {
+                motorOutputNormalised = 0;
+            }
+
+            motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
+
+            float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
+
+            // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
+            motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
+                    motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
+
+            motor[i] = motorOutput;
+        }
+    } else {
+        // Disarmed mode
+        stopMotors();
+    }
+}
 #endif
 
 void FAST_CODE writeMotors(void)
@@ -443,6 +527,13 @@ static int getReversibleMotorsThrottleDeadband(void)
 
 void FAST_CODE mixTable(const float dT)
 {
+#ifdef USE_DSHOT
+    if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+        applyFlipOverAfterCrashModeToMotors();
+        return;
+    }
+#endif
+
     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)) {
@@ -482,7 +573,7 @@ void FAST_CODE mixTable(const float dT)
     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) {
         throttleRangeMin = throttleIdleValue;
         throttleRangeMax = motorConfig()->maxthrottle;
-        mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); 
+        mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
     } else
 #endif
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
@@ -519,7 +610,7 @@ void FAST_CODE mixTable(const float dT)
         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
     #endif
         // Throttle compensation based on battery voltage
-        if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {                
+        if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
             mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
         }
     }

src/main/flight/mixer.h

@@ -92,6 +92,7 @@ typedef struct motorConfig_s {
     float throttleIdle;                     // Throttle IDLE value based on min_command, max_throttle, in percent
     float throttleScale;                    // Scaling factor for throttle.
     uint8_t motorPoleCount;                 // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
+    uint8_t flipOverAfterPowerFactor;       // Power factor from 0 to 100% of flip over after crash
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);

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;