Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

Added more mixer optimisations from #2898.

Browse source
This commit is contained in:
mikeller 2017-04-23 18:26:06 +12:00
parent 40a2bee492
commit 991e69974f
2 changed files with 31 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

61
src/main/flight/mixer.c
View file

@ -394,7 +394,7 @@ void mixerInit(mixerMode_e mixerMode)
initEscEndpoints(); initEscEndpoints();
} }
void pidInitMixer(struct pidProfile_s *pidProfile) void pidInitMixer(const struct pidProfile_s *pidProfile)
{ {
pidSumLimit = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit); pidSumLimit = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit);
pidSumLimitYaw = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw); pidSumLimitYaw = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw);
@ -560,17 +560,13 @@ void mixTable(pidProfile_t *pidProfile)
throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
const float motorOutputRange = motorOutputMax - motorOutputMin; const float motorOutputRange = motorOutputMax - motorOutputMin;
float scaledAxisPIDf[3];
// Calculate and Limit the PIDsum // Calculate and Limit the PIDsum
scaledAxisPIDf[FD_ROLL] = const float scaledAxisPidRoll =
constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit);
-pidSumLimit, pidSumLimit); const float scaledAxisPidPitch =
scaledAxisPIDf[FD_PITCH] = constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit);
constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, const float scaledAxisPidYaw =
-pidSumLimit, pidSumLimit); constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw);
scaledAxisPIDf[FD_YAW] =
constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING,
-pidSumLimitYaw, pidSumLimitYaw);
// Calculate voltage compensation // Calculate voltage compensation
const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
@ -578,21 +574,23 @@ void mixTable(pidProfile_t *pidProfile)
// Find roll/pitch/yaw desired output // Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS]; float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0; float motorMixMax = 0, motorMixMin = 0;
const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed);
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
motorMix[i] = float mix =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPidRoll * currentMixer[i].roll +
scaledAxisPIDf[ROLL] * currentMixer[i].roll + scaledAxisPidPitch * currentMixer[i].pitch +
scaledAxisPIDf[YAW] * currentMixer[i].yaw * -GET_DIRECTION(mixerConfig()->yaw_motors_reversed); scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection);
if (vbatCompensationFactor > 1.0f) { if (vbatCompensationFactor > 1.0f) {
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation mix *= vbatCompensationFactor; // Add voltage compensation
} }
if (motorMix[i] > motorMixMax) { if (mix > motorMixMax) {
motorMixMax = motorMix[i]; motorMixMax = mix;
} else if (motorMix[i] < motorMixMin) { } else if (mix < motorMixMin) {
motorMixMin = motorMix[i]; motorMixMin = mix;
} }
motorMix[i] = mix;
} }
motorMixRange = motorMixMax - motorMixMin; motorMixRange = motorMixMax - motorMixMin;
@ -607,42 +605,43 @@ void mixTable(pidProfile_t *pidProfile)
} }
} else { } else {
if (isAirmodeActive()) { // Only automatically adjust throttle during airmode scenario if (isAirmodeActive()) { // Only automatically adjust throttle during airmode scenario
float throttleLimitOffset = motorMixRange / 2.0f; const float throttleLimitOffset = motorMixRange / 2.0f;
throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
} }
} }
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
uint32_t i = 0; for (uint32_t i = 0; i < motorCount; i++) {
for (i = 0; i < motorCount; i++) { float motorOutput = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));
motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));
// Dshot works exactly opposite in lower 3D section. // Dshot works exactly opposite in lower 3D section.
if (mixerInversion) { if (mixerInversion) {
motor[i] = motorOutputMin + (motorOutputMax - motor[i]); motorOutput = motorOutputMin + (motorOutputMax - motorOutput);
} }
if (failsafeIsActive()) { if (failsafeIsActive()) {
if (isMotorProtocolDshot()) if (isMotorProtocolDshot()) {
motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range motorOutput = (motorOutput < motorOutputMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
}
motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); motorOutput = constrain(motorOutput, disarmMotorOutput, motorOutputMax);
} else { } else {
motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax); motorOutput = constrain(motorOutput, motorOutputMin, motorOutputMax);
} }
// Motor stop handling // Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
motor[i] = disarmMotorOutput; motorOutput = disarmMotorOutput;
} }
} }
motor[i] = motorOutput;
} }
// Disarmed mode // Disarmed mode
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i]; motor[i] = motor_disarmed[i];
} }
} }

2
src/main/flight/mixer.h
View file

@ -129,7 +129,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate);
void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerInit(mixerMode_e mixerMode); void mixerInit(mixerMode_e mixerMode);
struct pidProfile_s; struct pidProfile_s;
void pidInitMixer(struct pidProfile_s *pidProfile); void pidInitMixer(const struct pidProfile_s *pidProfile);
void mixerConfigureOutput(void); void mixerConfigureOutput(void);