Merge pull request #7026 from mikeller/fix_3d_crash_flip
Fixed crash flip with 3D enabled.
This commit is contained in:
commit
0ad3fbb5fc
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue