Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Update turtle / crashflip mode (#13905)

Browse source 
* 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 2024-10-05 07:58:33 +10:00 • committed by GitHub
parent 95d55525ad
commit 7156dc84a3
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
26 changed files with 247 additions and 155 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -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) },

4
src/main/cms/cms_menu_misc.c
View file

@ -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;
}
@ -161,6 +164,7 @@ static const OSD_Entry menuMiscEntries[]=
{ "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 },

108
src/main/fc/core.c
View file

@ -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);
}
// 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 {
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);
}
}
}
// 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 // 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)

2
src/main/fc/core.h
View file

@ -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);

2
src/main/fc/rc_modes.h
View file

@ -62,7 +62,7 @@ typedef enum {
BOXCAMERA1,
BOXCAMERA2,
BOXCAMERA3,
BOXFLIPOVERAFTERCRASH,
BOXCRASHFLIP,
BOXPREARM,
BOXBEEPGPSCOUNT,
BOXVTXPITMODE,

3
src/main/fc/runtime_config.h
View file

@ -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)

2
src/main/flight/gps_rescue.c
View file

@ -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

2
src/main/flight/imu.c
View file

@ -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;

113
src/main/flight/mixer.c
View file

@ -62,12 +62,16 @@
#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 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;
float FAST_DATA_ZERO_INIT motor[MAX_SUPPORTED_MOTORS];
@ -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,32 +274,36 @@ 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;
#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);
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;
signPitch = 0;
} else {
@ -303,7 +312,7 @@ static void applyFlipOverAfterCrashModeToMotors(void)
}
const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0;
const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(30 deg)
if (cosPhi < cosThreshold) {
// Enforce either roll or pitch exclusively, if not on diagonal
@ -314,10 +323,50 @@ static void applyFlipOverAfterCrashModeToMotors(void)
}
}
// 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;
// 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;
}
for (int i = 0; i < mixerRuntime.motorCount; ++i) {
float motorOutputNormalised =
@ -332,20 +381,18 @@ static void applyFlipOverAfterCrashModeToMotors(void)
motorOutputNormalised = 0;
}
}
motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
motorOutputNormalised = MIN(1.0f, crashflipPower * 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);
// set motors to disarm value when intended increase is less than deadband value
motorOutput = (motorOutputNormalised < CRASHFLIP_MOTOR_DEADBAND) ? mixerRuntime.disarmMotorOutput : motorOutput;
motor[i] = motorOutput;
}
} else {
// Disarmed mode
for (int i = 0; i < mixerRuntime.motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
// 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;
}

3
src/main/flight/mixer.h
View file

@ -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);

6
src/main/flight/mixer_init.c
View file

@ -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;

2
src/main/io/beeper.c
View file

@ -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") },

4
src/main/io/beeper.h
View file

@ -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) \

8
src/main/io/ledstrip.c
View file

@ -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:

4
src/main/msp/msp_box.c
View file

@ -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

6
src/main/osd/osd.c
View file

@ -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;
@ -1289,7 +1289,7 @@ void osdProcessStats2(timeUs_t currentTimeUs)
if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
// 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;

2
src/main/osd/osd.h
View file

@ -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,

2
src/main/osd/osd_elements.c
View file

@ -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) {

10
src/main/osd/osd_warnings.c
View file

@ -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;

2
src/main/osd/osd_warnings.h
View file

@ -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);

4
src/main/rx/rc_stats.c
View file

@ -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;

3
src/test/unit/arming_prevention_unittest.cc
View file

@ -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; }
}

2
src/test/unit/ledstrip_unittest.cc
View file

@ -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) { }

2
src/test/unit/link_quality_unittest.cc
View file

@ -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; }

2
src/test/unit/osd_unittest.cc
View file

@ -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; }

2
src/test/unit/vtx_unittest.cc
View file

@ -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; }
}