diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index df2fae9b71..fcf0ad606a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index e235a9d6b7..566fd2c51e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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