rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #7026 from mikeller/fix_3d_crash_flip 

Fixed crash flip with 3D enabled.
This commit is contained in:
Michael Keller 2018-11-07 13:04:49 +13:00 committed by GitHub
parent f729f3fc6d ff8b2aa451
commit 0ad3fbb5fc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 21 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/main/flight/mixer.c
View File

@ -381,13 +381,13 @@ void initEscEndpoints(void)
case PWM_TYPE_DSHOT150:
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (featureIsEnabled(FEATURE_3D)) {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
} else {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
}
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1;
break;
#endif
@ -575,7 +575,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) {
if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) {
// INVERTED
motorRangeMin = motorOutputLow;
motorRangeMax = deadbandMotor3dLow;
@ -697,23 +697,24 @@ static void applyFlipOverAfterCrashModeToMotors(void)
float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / flipStickRange;
for (int i = 0; i < motorCount; ++i) {
float motorOutput =
float motorOutputNormalised =
signPitch*currentMixer[i].pitch +
signRoll*currentMixer[i].roll +
signYaw*currentMixer[i].yaw;
if (motorOutput < 0) {
if (motorOutputNormalised < 0) {
if (mixerConfig()->crashflip_motor_percent > 0) {
motorOutput = -motorOutput * (float)mixerConfig()->crashflip_motor_percent / 100.0f;
motorOutputNormalised = -motorOutputNormalised * (float)mixerConfig()->crashflip_motor_percent / 100.0f;
} else {
motorOutput = disarmMotorOutput;
motorOutputNormalised = 0;
}
}
motorOutput = MIN(1.0f, flipPower * motorOutput);
motorOutput = motorOutputMin + motorOutput * motorOutputRange;
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) ? disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND);
motor[i] = motorOutput;
}
} else {
@ -798,8 +799,12 @@ void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
{
// Find min and max throttle based on conditions. Throttle has to be known before mixing
calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
if (isFlipOverAfterCrashActive()) {
applyFlipOverAfterCrashModeToMotors();
return;
}
@ -812,9 +817,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
}
#endif
// Find min and max throttle based on conditions. Throttle has to be known before mixing
calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
// Calculate and Limit the PID sum
const float scaledAxisPidRoll =
constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
@ -945,9 +947,9 @@ float convertExternalToMotor(uint16_t externalValue)
if (externalValue == PWM_RANGE_MID) {
motorValue = DSHOT_DISARM_COMMAND;
} else if (externalValue < PWM_RANGE_MID) {
motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
} else {
motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
} else {
motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_DISARM_COMMAND : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
@ -973,10 +975,10 @@ uint16_t convertMotorToExternal(float motorValue)
if (featureIsEnabled(FEATURE_3D)) {
if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) {
externalValue = PWM_RANGE_MID;
} else if (motorValue <= DSHOT_3D_DEADBAND_LOW) {
externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
} else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
} else {
externalValue = scaleRange(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
externalValue = scaleRange(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
}
} else {
externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);

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

@ -36,8 +36,7 @@
#define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48
#define DSHOT_MAX_THROTTLE 2047
#define DSHOT_3D_DEADBAND_LOW 1047
#define DSHOT_3D_DEADBAND_HIGH 1048
#define DSHOT_3D_FORWARD_MIN_THROTTLE 1048
// Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode