Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

extend crash-flip mode (#5163)

Browse source 
* extend crash-flip mode

* size optimizations

* adjust cosine threshold

* adjust deadband to 15%
This commit is contained in:
bkeinert 2018-03-03 23:37:01 +01:00 • committed by Michael Keller
parent 51f5c15e18
commit 5d3c3b766b
1 changed files with 44 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -626,21 +626,57 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
} }
#define CRASH_FLIP_DEADBAND 20 #define CRASH_FLIP_DEADBAND 20
#define CRASH_FLIP_STICK_MINF 0.15f
static void applyFlipOverAfterCrashModeToMotors(void) static void applyFlipOverAfterCrashModeToMotors(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
float motorMix[MAX_SUPPORTED_MOTORS]; float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH);
for (int i = 0; i < motorCount; i++) { float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL);
if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW);
motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1; 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(stickDeflectionPitchAbs*stickDeflectionPitchAbs + stickDeflectionRollAbs*stickDeflectionRollAbs);
if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
// If yaw is the dominant, disable pitch and roll
stickDeflectionLength = stickDeflectionYawAbs;
signRoll = 0;
signPitch = 0;
} else { } else {
motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1; // If pitch/roll dominant, disable yaw
signYaw = 0;
} }
// Apply the mix to motor endpoints
float motorOutput = motorOutputMin + motorOutputRange * motorMix[i]; float cosPhi = (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength);
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;
}
}
// Apply a reasonable amount of stick deadband
const float flipStickRange = 1.0f - CRASH_FLIP_STICK_MINF;
float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / flipStickRange;
for (int i = 0; i < motorCount; ++i) {
float motorOutput =
signPitch*currentMixer[i].pitch +
signRoll*currentMixer[i].roll +
signYaw*currentMixer[i].yaw;
motorOutput = MIN(1.0f, flipPower*motorOutput);
motorOutput = motorOutputMin + motorOutput*motorOutputRange;
// Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND; motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND);
motor[i] = motorOutput; motor[i] = motorOutput;
} }
} else { } else {