mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 06:45:16 +03:00
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
This commit is contained in:
ctzsnooze
GitHub
parent
95d55525ad
commit
7156dc84a3
26 changed files with 247 additions and 155 deletions
|
|
@ -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)) {
|
||||
// Set motor spin direction
|
||||
if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
|
||||
flipOverAfterCrashActive = false;
|
||||
if (!featureIsEnabled(FEATURE_3D)) {
|
||||
dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, DSHOT_CMD_TYPE_INLINE);
|
||||
}
|
||||
} else {
|
||||
flipOverAfterCrashActive = true;
|
||||
#ifdef USE_RUNAWAY_TAKEOFF
|
||||
runawayTakeoffCheckDisabled = false;
|
||||
#endif
|
||||
if (!featureIsEnabled(FEATURE_3D)) {
|
||||
dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, DSHOT_CMD_TYPE_INLINE);
|
||||
}
|
||||
// choose crashflip outcome on arming
|
||||
// disarm can arise in processRx() if the crashflip switch is reversed while in crashflip mode
|
||||
// if we were unsuccessful, or cannot determin success, arming will be blocked and we can't get here
|
||||
// hence we only get here with crashFlipModeActive if the switch was reversed and result successful
|
||||
if (crashFlipModeActive) {
|
||||
// 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 {
|
||||
// when arming and not in crashflip mode, block entry to crashflip if delayed by the dshot beeper,
|
||||
// otherwise consider only the switch position
|
||||
crashFlipModeActive = (tryingToArm == ARMING_DELAYED_CRASHFLIP) ? false : IS_RC_MODE_ACTIVE(BOXCRASHFLIP);
|
||||
#ifdef USE_DSHOT
|
||||
// previous disarm will have set direction to normal
|
||||
// at this point we only need to reverse the motors if crashflipMode is active
|
||||
if (crashFlipModeActive) {
|
||||
setMotorSpinDirection(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#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 (flipOverAfterCrashActive) {
|
||||
beeper(BEEPER_CRASH_FLIP_MODE);
|
||||
if (crashFlipModeActive) {
|
||||
// Enable beep warning when the crashflip mode is active
|
||||
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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue