From ea99f7785b974a473c7f042e82f48b6657e6a6d8 Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 2 Nov 2018 01:10:36 +1300 Subject: [PATCH 1/2] Fixed crash flip with 3D enabled. --- src/main/flight/mixer.c | 45 ++++++++++++++++++++++++++--------------- src/main/flight/mixer.h | 3 +-- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index df2fae9b71..c7f51e2f74 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 @@ -697,23 +697,36 @@ 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; + + if (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) { + motorOutput = disarmMotorOutput; + } else { + // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered + motorOutput = motorOutput - CRASH_FLIP_DEADBAND; + + if (featureIsEnabled(FEATURE_3D)) { + if (motorOutput < DSHOT_3D_FORWARD_MIN_THROTTLE) { + motorOutput = motorOutput + DSHOT_3D_FORWARD_MIN_THROTTLE; + } else { + motorOutput = motorOutput - DSHOT_3D_FORWARD_MIN_THROTTLE; + } + } + } - // 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 { @@ -945,9 +958,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 +986,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 From ff8b2aa451eafa5bcb08492a8db7eb0fbaeea87d Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 7 Nov 2018 02:52:40 +1300 Subject: [PATCH 2/2] Fixed '3d_switched_mode' case. --- src/main/flight/mixer.c | 25 +++++++------------------ 1 file changed, 7 insertions(+), 18 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c7f51e2f74..fcf0ad606a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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; @@ -712,20 +712,8 @@ static void applyFlipOverAfterCrashModeToMotors(void) motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange; - if (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) { - motorOutput = disarmMotorOutput; - } else { - // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered - motorOutput = motorOutput - CRASH_FLIP_DEADBAND; - - if (featureIsEnabled(FEATURE_3D)) { - if (motorOutput < DSHOT_3D_FORWARD_MIN_THROTTLE) { - motorOutput = motorOutput + DSHOT_3D_FORWARD_MIN_THROTTLE; - } else { - motorOutput = motorOutput - DSHOT_3D_FORWARD_MIN_THROTTLE; - } - } - } + // 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; } @@ -811,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; } @@ -825,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;