From 7156dc84a39f0068b45d6afa82ccd3afa6fc6cb6 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 5 Oct 2024 07:58:33 +1000 Subject: [PATCH] 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 --- src/main/cli/settings.c | 2 +- src/main/cms/cms_menu_misc.c | 14 +- src/main/fc/core.c | 108 +++++++---- src/main/fc/core.h | 2 +- src/main/fc/rc_modes.h | 2 +- src/main/fc/runtime_config.h | 3 +- src/main/flight/gps_rescue.c | 2 +- src/main/flight/imu.c | 2 +- src/main/flight/mixer.c | 195 ++++++++++++-------- src/main/flight/mixer.h | 3 +- src/main/flight/mixer_init.c | 6 +- src/main/io/beeper.c | 2 +- src/main/io/beeper.h | 4 +- src/main/io/ledstrip.c | 8 +- src/main/msp/msp_box.c | 4 +- src/main/osd/osd.c | 8 +- src/main/osd/osd.h | 2 +- src/main/osd/osd_elements.c | 2 +- src/main/osd/osd_warnings.c | 10 +- src/main/osd/osd_warnings.h | 2 +- src/main/rx/rc_stats.c | 4 +- src/test/unit/arming_prevention_unittest.cc | 3 +- src/test/unit/ledstrip_unittest.cc | 2 +- src/test/unit/link_quality_unittest.cc | 2 +- src/test/unit/osd_unittest.cc | 2 +- src/test/unit/vtx_unittest.cc | 2 + 26 files changed, 244 insertions(+), 152 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a831427bd9b..e6c53e78964 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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) }, diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 80bcb12c27f..8c491601e83 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -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) { @@ -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; } @@ -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; } @@ -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}, diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 3b9bcd16b97..28d0ae7df0e 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -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) diff --git a/src/main/fc/core.h b/src/main/fc/core.h index ad6f4a27bf3..baa76c30bfb 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -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); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index dc0a4c126ed..0fe33c5b46b 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -62,7 +62,7 @@ typedef enum { BOXCAMERA1, BOXCAMERA2, BOXCAMERA3, - BOXFLIPOVERAFTERCRASH, + BOXCRASHFLIP, BOXPREARM, BOXBEEPGPSCOUNT, BOXVTXPITMODE, diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index ae73853f4dd..ea975b78aec 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -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) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 3d0277e424f..74ca83ab3d8 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -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 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ac699a577b8..ebead68fd5e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c2623ae09bb..7d8fff3f54b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -62,11 +62,15 @@ #include "sensors/battery.h" #include "sensors/gyro.h" +#include "sensors/sensors.h" #include "mixer.h" -#define DYN_LPF_THROTTLE_STEPS 100 -#define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates +#define DYN_LPF_THROTTLE_STEPS 100 +#define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates + +#define CRASHFLIP_MOTOR_DEADBAND 0.02f // 2%; send 'disarm' value to motors below this drive value +#define CRASHFLIP_STICK_DEADBAND 0.15f // 15% static FAST_DATA_ZERO_INIT float motorMixRange; @@ -108,6 +112,7 @@ static FAST_DATA_ZERO_INIT float motorRangeMin; static FAST_DATA_ZERO_INIT float motorRangeMax; static FAST_DATA_ZERO_INIT float motorOutputRange; static FAST_DATA_ZERO_INIT int8_t motorOutputMixSign; +static FAST_DATA_ZERO_INIT bool crashflipSuccess = false; static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { @@ -139,7 +144,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; - if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) { + if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isCrashFlipModeActive()) { // INVERTED motorRangeMin = mixerRuntime.motorOutputLow; motorRangeMax = mixerRuntime.deadbandMotor3dLow; @@ -255,7 +260,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100)); DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000)); } - motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); + motorRangeMax = isCrashFlipModeActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); #else motorRangeMax = mixerRuntime.motorOutputHigh; #endif @@ -269,83 +274,125 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) rcThrottle = throttle; } -#define CRASH_FLIP_DEADBAND 20 -#define CRASH_FLIP_STICK_MINF 0.15f - -static void applyFlipOverAfterCrashModeToMotors(void) +static bool applyCrashFlipModeToMotors(void) { - if (ARMING_FLAG(ARMED)) { - const float flipPowerFactor = 1.0f - mixerConfig()->crashflip_expo / 100.0f; - const float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH); - const float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL); - const float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW); - - 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 = getRcDeflection(FD_PITCH) < 0 ? 1 : -1; - float signRoll = getRcDeflection(FD_ROLL) < 0 ? 1 : -1; - float signYaw = (getRcDeflection(FD_YAW) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 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; +#ifdef USE_ACC + static bool isTiltAngleAtStartSet = false; + static float tiltAngleAtStart = 1.0f; +#endif + + if (!isCrashFlipModeActive()) { +#ifdef USE_ACC + // trigger the capture of initial tilt angle on next activation of crashflip mode + isTiltAngleAtStartSet = false; + // default the success flag to false, to block quick re-arming unless successful +#endif + // signal that crashflip mode is off + return false; + } + + const float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH); + const float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL); + const float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW); + + float signPitch = getRcDeflection(FD_PITCH) < 0 ? 1 : -1; + float signRoll = getRcDeflection(FD_ROLL) < 0 ? 1 : -1; + float signYaw = (getRcDeflection(FD_YAW) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1); + + float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); + + if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) { + // If yaw is the dominant, disable pitch and roll + stickDeflectionLength = stickDeflectionYawAbs; + 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(30 deg) + + if (cosPhi < cosThreshold) { + // Enforce either roll or pitch exclusively, if not on diagonal + if (stickDeflectionRollAbs > stickDeflectionPitchAbs) { signPitch = 0; } else { - // If pitch/roll dominant, disable yaw - signYaw = 0; + signRoll = 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; + // Calculate crashflipPower from stick deflection with a reasonable amount of stick deadband + float crashflipPower = stickDeflectionLength > CRASHFLIP_STICK_DEADBAND ? stickDeflectionLength : 0.0f; + + // calculate flipPower attenuators + float crashflipRateAttenuator = 1.0f; + float crashflipAttitudeAttenuator = 1.0f; + const float crashflipRateLimit = mixerConfig()->crashflip_rate * 10.0f; // eg 35 = no power by 350 deg/s + const float halfComplete = 0.5f; // attitude or rate changes less that this will be ignored + + // disable both attenuators if the user's crashflip_rate is zero + if (crashflipRateLimit > 0) { +#ifdef USE_ACC + // Calculate an attenuator based on change of attitude (requires Acc) + // with Acc, crashflipAttitudeAttenuator will be zero after approx 90 degree rotation, and + // motors will stop / not spin while attitude remains more than ~90 degrees from initial attitude + // without Acc, the user must manually center the stick, or exit crashflip mode, or disarm, to stop the motors + // re-initialisation of crashFlip mode by arm/disarm is required to reset the initial tilt angle + if (sensors(SENSOR_ACC)) { + const float tiltAngle = getCosTiltAngle(); // -1 if flat inverted, 0 when 90° (on edge), +1 when flat and upright + if (!isTiltAngleAtStartSet) { + tiltAngleAtStart = tiltAngle; + isTiltAngleAtStartSet = true; + crashflipSuccess = false; } + // attitudeChangeNeeded is 1.0 at the start, decreasing to 0 when attitude change exceeds approx 90 degrees + const float attitudeChangeNeeded = fmaxf(1.0f - fabsf(tiltAngle - tiltAngleAtStart), 0.0f); + // no attenuation unless a significant attitude change has occurred + crashflipAttitudeAttenuator = attitudeChangeNeeded > halfComplete ? 1.0f : attitudeChangeNeeded / halfComplete; + + // signal success to enable quick restart, if attitude change implies success when reverting the switch + crashflipSuccess = attitudeChangeNeeded == 0.0f; } +#endif // USE_ACC + // Calculate an attenuation factor based on rate of rotation... note: + // if driving roll or pitch, quad usually turns on that axis, but if one motor sticks, could be a diagonal rotation + // if driving diagonally, the turn could be either roll or pitch + // if driving yaw, typically one motor sticks, and the quad yaws a little then flips diagonally + const float gyroRate = fmaxf(fabsf(gyro.gyroADCf[FD_ROLL]), fabsf(gyro.gyroADCf[FD_PITCH])); + const float gyroRateChange = fminf(gyroRate / crashflipRateLimit, 1.0f); + // no attenuation unless a significant gyro rate change has occurred + crashflipRateAttenuator = gyroRateChange < halfComplete ? 1.0f : (1.0f - gyroRateChange) / halfComplete; + + crashflipPower *= crashflipAttitudeAttenuator * crashflipRateAttenuator; + } - // Apply a reasonable amount of stick deadband - const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); - const float flipStickRange = 1.0f - crashFlipStickMinExpo; - const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; - - for (int i = 0; i < mixerRuntime.motorCount; ++i) { - float motorOutputNormalised = - signPitch * mixerRuntime.currentMixer[i].pitch + - signRoll * mixerRuntime.currentMixer[i].roll + - signYaw * mixerRuntime.currentMixer[i].yaw; - - if (motorOutputNormalised < 0) { - if (mixerConfig()->crashflip_motor_percent > 0) { - motorOutputNormalised = -motorOutputNormalised * (float)mixerConfig()->crashflip_motor_percent / 100.0f; - } else { - motorOutputNormalised = 0; - } + for (int i = 0; i < mixerRuntime.motorCount; ++i) { + float motorOutputNormalised = + signPitch * mixerRuntime.currentMixer[i].pitch + + signRoll * mixerRuntime.currentMixer[i].roll + + signYaw * mixerRuntime.currentMixer[i].yaw; + + if (motorOutputNormalised < 0) { + if (mixerConfig()->crashflip_motor_percent > 0) { + motorOutputNormalised = -motorOutputNormalised * (float)mixerConfig()->crashflip_motor_percent / 100.0f; + } else { + motorOutputNormalised = 0; } - motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); - float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange; + } - // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered - motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? mixerRuntime.disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND); + motorOutputNormalised = MIN(1.0f, crashflipPower * motorOutputNormalised); + float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange; - motor[i] = motorOutput; - } - } else { - // Disarmed mode - for (int i = 0; i < mixerRuntime.motorCount; i++) { - motor[i] = motor_disarmed[i]; - } + // set motors to disarm value when intended increase is less than deadband value + motorOutput = (motorOutputNormalised < CRASHFLIP_MOTOR_DEADBAND) ? mixerRuntime.disarmMotorOutput : motorOutput; + + motor[i] = motorOutput; } + + // signal that crashflip mode has been applied to motors + return true; } #ifdef USE_RPM_LIMIT @@ -607,9 +654,8 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(currentTimeUs); - if (isFlipOverAfterCrashActive()) { - applyFlipOverAfterCrashModeToMotors(); - return; + if (applyCrashFlipModeToMotors()) { + return; // if crash flip mode has been applied to the motors, mixing is done } const bool launchControlActive = isLaunchControlActive(); @@ -786,3 +832,8 @@ float mixerGetRcThrottle(void) { return rcThrottle; } + +bool crashFlipSuccessful(void) +{ + return crashflipSuccess; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index d231438c7b9..9234db75f87 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -95,7 +95,7 @@ typedef struct mixerConfig_s { uint8_t mixerMode; bool yaw_motors_reversed; uint8_t crashflip_motor_percent; - uint8_t crashflip_expo; + uint8_t crashflip_rate; uint8_t mixer_type; #ifdef USE_RPM_LIMIT bool rpm_limit; @@ -147,3 +147,4 @@ bool isFixedWing(void); float getMotorOutputLow(void); float getMotorOutputHigh(void); +bool crashFlipSuccessful(void); diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index 22fef63fce2..a223f341a27 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -54,7 +54,11 @@ void pgResetFn_mixerConfig(mixerConfig_t *mixerConfig) mixerConfig->mixerMode = DEFAULT_MIXER; mixerConfig->yaw_motors_reversed = false; mixerConfig->crashflip_motor_percent = 0; - mixerConfig->crashflip_expo = 35; +#ifdef USE_RACE_PRO + mixerConfig->crashflip_rate = 30; +#else + mixerConfig->crashflip_rate = 0; +#endif mixerConfig->mixer_type = MIXER_LEGACY; #ifdef USE_RPM_LIMIT mixerConfig->rpm_limit = false; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 584a5fc06db..89a5b341357 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -230,7 +230,7 @@ static const beeperTableEntry_t beeperTable[] = { { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") }, { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 19, beep_2shortBeeps, "BLACKBOX_ERASE") }, - { BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 20, beep_2longerBeeps, "CRASH_FLIP") }, + { BEEPER_ENTRY(BEEPER_CRASHFLIP_MODE, 20, beep_2longerBeeps, "CRASHFLIP") }, { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSE") }, { BEEPER_ENTRY(BEEPER_RC_SMOOTHING_INIT_FAIL,23, beep_rcSmoothingInitFail, "RC_SMOOTHING_INIT_FAIL") }, diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 97da104da29..33c9fdcd73f 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -56,7 +56,7 @@ typedef enum { BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes - BEEPER_CRASH_FLIP_MODE, // Crash flip mode is active + BEEPER_CRASHFLIP_MODE, // Crashflip mode is active BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop BEEPER_RC_SMOOTHING_INIT_FAIL, // Warning beep pattern when armed and rc smoothing has not initialized filters @@ -87,7 +87,7 @@ STATIC_ASSERT(BEEPER_ALL < sizeof(uint32_t) * 8, "BEEPER bits exhausted"); | BEEPER_GET_FLAG(BEEPER_SYSTEM_INIT) \ | BEEPER_GET_FLAG(BEEPER_USB) \ | BEEPER_GET_FLAG(BEEPER_BLACKBOX_ERASE) \ - | BEEPER_GET_FLAG(BEEPER_CRASH_FLIP_MODE) \ + | BEEPER_GET_FLAG(BEEPER_CRASHFLIP_MODE) \ | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_OPEN) \ | BEEPER_GET_FLAG(BEEPER_CAM_CONNECTION_CLOSE) \ | BEEPER_GET_FLAG(BEEPER_RC_SMOOTHING_INIT_FAIL) \ diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index fa6deeaafbf..9718ae7e469 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -690,7 +690,7 @@ typedef enum { WARNING_ARMING_DISABLED, WARNING_LOW_BATTERY, WARNING_FAILSAFE, - WARNING_CRASH_FLIP_ACTIVE, + WARNING_CRASHFLIP_ACTIVE, } warningFlags_e; static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) @@ -714,8 +714,8 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) if (!ARMING_FLAG(ARMED) && isArmingDisabled()) { warningFlags |= 1 << WARNING_ARMING_DISABLED; } - if (isFlipOverAfterCrashActive()) { - warningFlags |= 1 << WARNING_CRASH_FLIP_ACTIVE; + if (isCrashFlipModeActive()) { + warningFlags |= 1 << WARNING_CRASHFLIP_ACTIVE; } } *timer += HZ_TO_US(LED_OVERLAY_WARNING_RATE_HZ); @@ -731,7 +731,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer) case WARNING_ARMING_DISABLED: warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK); break; - case WARNING_CRASH_FLIP_ACTIVE: + case WARNING_CRASHFLIP_ACTIVE: warningColor = colorOn ? &HSV(MAGENTA) : &HSV(BLACK); break; case WARNING_LOW_BATTERY: diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index ba70f2dab5f..b06de04a722 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -82,7 +82,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { .boxId = BOXCAMERA1, .boxName = "CAMERA CONTROL 1", .permanentId = 32}, { .boxId = BOXCAMERA2, .boxName = "CAMERA CONTROL 2", .permanentId = 33}, { .boxId = BOXCAMERA3, .boxName = "CAMERA CONTROL 3", .permanentId = 34 }, - { .boxId = BOXFLIPOVERAFTERCRASH, .boxName = "FLIP OVER AFTER CRASH", .permanentId = 35 }, + { .boxId = BOXCRASHFLIP, .boxName = "FLIP OVER AFTER CRASH", .permanentId = 35 }, { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 36 }, { .boxId = BOXBEEPGPSCOUNT, .boxName = "GPS BEEP SATELLITE COUNT", .permanentId = 37 }, // { .boxId = BOX3DONASWITCH, .boxName = "3D ON A SWITCH", .permanentId = 38 }, (removed) @@ -283,7 +283,7 @@ void initActiveBoxIds(void) bool configuredMotorProtocolDshot; checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot); if (configuredMotorProtocolDshot) { - BME(BOXFLIPOVERAFTERCRASH); + BME(BOXCRASHFLIP); } #endif diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 6b5df5b7641..56b52cf32a4 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -1201,8 +1201,8 @@ static timeDelta_t osdShowArmed(void) } displayWrite(osdDisplayPort, midCol - (strlen("ARMED") / 2), midRow, DISPLAYPORT_SEVERITY_NORMAL, "ARMED"); - if (isFlipOverAfterCrashActive()) { - displayWrite(osdDisplayPort, midCol - (strlen(CRASH_FLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASH_FLIP_WARNING); + if (isCrashFlipModeActive()) { + displayWrite(osdDisplayPort, midCol - (strlen(CRASHFLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASHFLIP_WARNING); } return ret; @@ -1287,9 +1287,9 @@ void osdProcessStats2(timeUs_t currentTimeUs) if (resumeRefreshAt) { if (cmp32(currentTimeUs, resumeRefreshAt) < 0) { - // in timeout period, check sticks for activity or CRASH FLIP switch to resume display. + // in timeout period, check sticks for activity or CRASHFLIP switch to resume display. if (!ARMING_FLAG(ARMED) && - (IS_HI(THROTTLE) || IS_HI(PITCH) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH))) { + (IS_HI(THROTTLE) || IS_HI(PITCH) || IS_RC_MODE_ACTIVE(BOXCRASHFLIP))) { resumeRefreshAt = currentTimeUs; } return; diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index e89bbf88352..e6ae3448bf8 100644 --- a/src/main/osd/osd.h +++ b/src/main/osd/osd.h @@ -269,7 +269,7 @@ typedef enum { OSD_WARNING_BATTERY_WARNING, OSD_WARNING_BATTERY_CRITICAL, OSD_WARNING_VISUAL_BEEPER, - OSD_WARNING_CRASH_FLIP, + OSD_WARNING_CRASHFLIP, OSD_WARNING_ESC_FAIL, OSD_WARNING_CORE_TEMPERATURE, OSD_WARNING_RC_SMOOTHING, diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index e3b5a8cdfc9..9cf07b4a3f6 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -887,7 +887,7 @@ static void osdElementCrashFlipArrow(osdElementParms_t *element) rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle; } - if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) { + if ((isCrashFlipModeActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) { if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) { if (pitchAngle > 0) { if (rollAngle > 0) { diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c index 4b5e57ccb28..3f46435056a 100644 --- a/src/main/osd/osd_warnings.c +++ b/src/main/osd/osd_warnings.c @@ -64,7 +64,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" -const char CRASH_FLIP_WARNING[] = "> CRASH FLIP <"; +const char CRASHFLIP_WARNING[] = "> CRASH FLIP <"; void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr) { @@ -135,12 +135,12 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr) } // Warn when in flip over after crash mode - if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { - if (isFlipOverAfterCrashActive()) { // if was armed in crash flip mode - tfp_sprintf(warningText, CRASH_FLIP_WARNING); + if (osdWarnGetState(OSD_WARNING_CRASHFLIP) && IS_RC_MODE_ACTIVE(BOXCRASHFLIP)) { + if (isCrashFlipModeActive()) { // if was armed in crashflip mode + tfp_sprintf(warningText, CRASHFLIP_WARNING); *displayAttr = DISPLAYPORT_SEVERITY_INFO; return; - } else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crash flip mode is activated + } else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crashflip mode is activated (not allowed / can't happen) tfp_sprintf(warningText, "CRASH FLIP SWITCH"); *displayAttr = DISPLAYPORT_SEVERITY_INFO; return; diff --git a/src/main/osd/osd_warnings.h b/src/main/osd/osd_warnings.h index 393ea8b26a4..edf928f038b 100644 --- a/src/main/osd/osd_warnings.h +++ b/src/main/osd/osd_warnings.h @@ -23,7 +23,7 @@ #define OSD_WARNINGS_MAX_SIZE 12 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1) -extern const char CRASH_FLIP_WARNING[]; +extern const char CRASHFLIP_WARNING[]; STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= OSD_ELEMENT_BUFFER_LENGTH, osd_warnings_size_exceeds_buffer_size); diff --git a/src/main/rx/rc_stats.c b/src/main/rx/rc_stats.c index 5968fd06c8f..aa189a543a5 100644 --- a/src/main/rx/rc_stats.c +++ b/src/main/rx/rc_stats.c @@ -49,13 +49,13 @@ void rcStatsUpdate(timeUs_t currentTimeUs) previousTimeUs = currentTimeUs; const int8_t throttlePercent = calculateThrottlePercent(); - if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && !throttleEverRaisedAfterArming) { + if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXCRASHFLIP) && !throttleEverRaisedAfterArming) { if (abs(throttlePercent) >= 15) { // start counting stats if throttle was raised >= 15% after arming throttleEverRaisedAfterArming = true; } } - if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && throttleEverRaisedAfterArming) { + if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXCRASHFLIP) && throttleEverRaisedAfterArming) { counter++; totalTrottleNumber += throttlePercent; diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 39d9f39d88e..44146f495f5 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1161,5 +1161,6 @@ extern "C" { } void getRcDeflectionAbs(void) {} uint32_t getCpuPercentageLate(void) { return 0; } - bool isAltitudeLow(void) {return false ;}; + bool crashFlipSuccessful(void) {return false; } + bool isAltitudeLow(void) {return false; } } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 40de5f07bf5..da9fc21ef79 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -403,7 +403,7 @@ bool isArmingDisabled(void) { return false; } uint8_t getRssiPercent(void) { return 0; } -bool isFlipOverAfterCrashActive(void) { return false; } +bool isCrashFlipModeActive(void) { return false; } void ws2811LedStripEnable(void) { } diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 4f96893bfe1..c7eb278496a 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -494,7 +494,7 @@ extern "C" { bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} bool cmsDisplayPortRegister(displayPort_t *) { return false; } uint16_t getCoreTemperatureCelsius(void) { return 0; } - bool isFlipOverAfterCrashActive(void) { return false; } + bool isCrashFlipModeActive(void) { return false; } float pidItermAccelerator(void) { return 1.0; } uint8_t getMotorCount(void){ return 4; } bool areMotorsRunning(void){ return true; } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 4749612d948..40ebe0a0743 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1396,7 +1396,7 @@ extern "C" { uint16_t getCoreTemperatureCelsius(void) { return simulationCoreTemperature; } - bool isFlipOverAfterCrashActive(void) { return false; } + bool isCrashFlipModeActive(void) { return false; } float pidItermAccelerator(void) { return 1.0; } uint8_t getMotorCount(void){ return 4; } diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 8da67bdc8d9..77068fc0706 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -211,4 +211,6 @@ extern "C" { void sbufWriteU16(sbuf_t *, uint16_t) {} void sbufWriteU32(sbuf_t *, uint32_t) {} void schedulerSetNextStateTime(timeDelta_t) {} + bool crashFlipSuccessful(void) {return false; } + }