Skip to content

Commit

Permalink
Update turtle / crashflip mode (betaflight#13905)
Browse files Browse the repository at this point in the history
* 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
  • Loading branch information
ctzsnooze authored Oct 4, 2024
1 parent 95d5552 commit 7156dc8
Show file tree
Hide file tree
Showing 26 changed files with 244 additions and 152 deletions.
2 changes: 1 addition & 1 deletion src/main/cli/settings.c
Original file line number Diff line number Diff line change
Expand Up @@ -958,7 +958,7 @@ const clivalue_t valueTable[] = {
{ "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) },
{ PARAM_NAME_MIXER_TYPE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) },
{ "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) },
{ "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) },
{ "crashflip_rate", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_rate) },
#ifdef USE_RPM_LIMIT
{ "rpm_limit", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit) },
{ "rpm_limit_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_p) },
Expand Down
14 changes: 9 additions & 5 deletions src/main/cms/cms_menu_misc.c
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,7 @@ CMS_Menu cmsx_menuRcPreview = {
static uint16_t motorConfig_minthrottle;
static uint8_t motorConfig_digitalIdleOffsetValue;
static uint8_t rxConfig_fpvCamAngleDegrees;
static uint8_t mixerConfig_crashflip_rate;

static const void *cmsx_menuMiscOnEnter(displayPort_t *pDisp)
{
Expand All @@ -138,6 +139,7 @@ static const void *cmsx_menuMiscOnEnter(displayPort_t *pDisp)
motorConfig_minthrottle = motorConfig()->minthrottle;
motorConfig_digitalIdleOffsetValue = motorConfig()->digitalIdleOffsetValue / 10;
rxConfig_fpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
mixerConfig_crashflip_rate = mixerConfig()->crashflip_rate;

return NULL;
}
Expand All @@ -150,6 +152,7 @@ static const void *cmsx_menuMiscOnExit(displayPort_t *pDisp, const OSD_Entry *se
motorConfigMutable()->minthrottle = motorConfig_minthrottle;
motorConfigMutable()->digitalIdleOffsetValue = 10 * motorConfig_digitalIdleOffsetValue;
rxConfigMutable()->fpvCamAngleDegrees = rxConfig_fpvCamAngleDegrees;
mixerConfigMutable()->crashflip_rate = mixerConfig_crashflip_rate;

return NULL;
}
Expand All @@ -158,12 +161,13 @@ static const OSD_Entry menuMiscEntries[]=
{
{ "-- MISC --", OME_Label, NULL, NULL },

{ "MIN THR", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 } },
{ "DIGITAL IDLE", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &motorConfig_digitalIdleOffsetValue, 0, 200, 1 } },
{ "FPV CAM ANGLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &rxConfig_fpvCamAngleDegrees, 0, 90, 1 } },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview},
{ "MIN THR", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 } },
{ "DIGITAL IDLE", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &motorConfig_digitalIdleOffsetValue, 0, 200, 1 } },
{ "FPV CAM ANGLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &rxConfig_fpvCamAngleDegrees, 0, 90, 1 } },
{ "CRASHFLIP RATE", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t) { &mixerConfig_crashflip_rate, 0, 100, 1 } },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview},
#ifdef USE_GPS_LAP_TIMER
{ "GPS LAP TIMER", OME_Submenu, cmsMenuChange, &cms_menuGpsLapTimer },
{ "GPS LAP TIMER", OME_Submenu, cmsMenuChange, &cms_menuGpsLapTimer },
#endif // USE_GPS_LAP_TIMER

{ "BACK", OME_Back, NULL, NULL},
Expand Down
108 changes: 68 additions & 40 deletions src/main/fc/core.c
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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
Expand All @@ -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))) {
Expand All @@ -488,6 +508,7 @@ void tryArm(void)
gyroStartCalibration(true);
}

// runs each loop while arming switches are engaged
updateArmingStatus();

if (!isArmingDisabled()) {
Expand All @@ -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()) {
Expand All @@ -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;
}
Expand Down Expand Up @@ -807,7 +831,7 @@ bool processRx(timeUs_t currentTimeUs)
if (ARMING_FLAG(ARMED)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashActive
&& !crashFlipModeActive
&& !runawayTakeoffTemporarilyDisabled
&& !isFixedWing()) {

Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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))) {
Expand Down Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/core.h
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ bool pidLoopReady(void);
void taskFiltering(timeUs_t currentTimeUs);
void taskMainPidLoop(timeUs_t currentTimeUs);

bool isFlipOverAfterCrashActive(void);
bool isCrashFlipModeActive(void);
int8_t calculateThrottlePercent(void);
uint8_t calculateThrottlePercentAbs(void);
bool areSticksActive(uint8_t stickPercentLimit);
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/rc_modes.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ typedef enum {
BOXCAMERA1,
BOXCAMERA2,
BOXCAMERA3,
BOXFLIPOVERAFTERCRASH,
BOXCRASHFLIP,
BOXPREARM,
BOXBEEPGPSCOUNT,
BOXVTXPITMODE,
Expand Down
3 changes: 2 additions & 1 deletion src/main/fc/runtime_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,8 @@ typedef enum {
ARMING_DISABLED_DSHOT_BITBANG = (1 << 22),
ARMING_DISABLED_ACC_CALIBRATION = (1 << 23),
ARMING_DISABLED_MOTOR_PROTOCOL = (1 << 24),
ARMING_DISABLED_ARM_SWITCH = (1 << 25), // Needs to be the last element, since it's always activated if one of the others is active when arming
ARMING_DISABLED_CRASHFLIP = (1 << 25),
ARMING_DISABLED_ARM_SWITCH = (1 << 26), // Needs to be the last element, since it's always activated if one of the others is active when arming
} armingDisableFlags_e;

#define ARMING_DISABLE_FLAGS_COUNT (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1)
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/gps_rescue.c
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ typedef enum {
RESCUE_FLYAWAY,
RESCUE_GPSLOST,
RESCUE_LOWSATS,
RESCUE_CRASH_FLIP_DETECTED,
RESCUE_CRASHFLIP_DETECTED,
RESCUE_STALLED,
RESCUE_TOO_CLOSE,
RESCUE_NO_HOME_POINT
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/imu.c
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ static vector2_t north_ef;

#if defined(USE_ACC)
STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
#endif
#endif // USE_ACC

// quaternion of sensor frame relative to earth frame
STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
Expand Down
Loading

0 comments on commit 7156dc8

Please sign in to comment.