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

Fixed crash flip with 3D enabled.

This commit is contained in:
mikeller 2018-11-02 01:10:36 +13:00
parent 56b1c08561
commit ea99f7785b
2 changed files with 30 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
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
@ -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);

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