diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0a90d5a73f..6a37dadaae 100644 --- a/src/main/fc/fc_core.c +++ b/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; +} diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 085ce3d758..d9ac269c69 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -49,3 +49,4 @@ void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); bool isMotorsReversed(void); +bool isFlipOverAfterCrashMode(void); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 769d1c7e13..ee87442e73 100644 --- a/src/main/fc/fc_msp_box.c +++ b/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 diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 25658e9fdb..500103c4f2 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -57,7 +57,7 @@ typedef enum { BOXCAMERA1, BOXCAMERA2, BOXCAMERA3, - BOXDSHOTREVERSE, + BOXFLIPOVERAFTERCRASH, BOXPREARM, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 0cf6465d42..73c8af4b6d 100755 --- a/src/main/flight/mixer.c +++ b/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();